diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py
index df6bd67..60d6d96 100644
--- a/src/OptAlgorithm/OptTimeCalculator.py
+++ b/src/OptAlgorithm/OptTimeCalculator.py
@@ -175,7 +175,7 @@ class OptTimeCalculator(AutoConfigClass):
         t31 = v0 / a + t1
         t5max = (Tfull - v0 / a) / 2 - t1
         v1 = v0 + a * t1
-        S1 = v0 * t1 + a * t1 * t1 / 2 + v1 * t31 - a * t31 * t31 / 2
+        S1 = v0 * t1 + a * t1 * t1 / 2 + v1 * t31 - a * t31 * t31 / 2 + v0 * t31
         S2max = -Sfull + S1
         t5 = min(t5max, (vmax) / a, sqrt(S2max / a))
         t3 = abs(v0) / a + t1 + t5