Diff: trapezoidalMotionCal.cpp

Revision:

2:a43e90cb271c

Parent:

1:15adfb7ee64c

diff -r 15adfb7ee64c -r a43e90cb271c trapezoidalMotionCal.cpp
--- a/trapezoidalMotionCal.cpp	Thu Aug 03 12:42:12 2017 +0000
+++ b/trapezoidalMotionCal.cpp	Thu Sep 07 06:42:08 2017 +0000
@@ -24,17 +24,19 @@
         targSteps *= -1;
     }
     
-    step_accel = (_f_max * _f_max - _f_start * _f_start) / (2 * _a_up);
-    step_const = (double)targSteps - step_accel - step_decel;
-    step_decel = (_f_stop * _f_stop - _f_max * _f_max) / (2 * _a_down);
-    
     if (step_const <= 0) {
         shortMode = 1;
         step_accel = (2 * _a_down * targSteps + _f_start * _f_start - _f_stop * _f_stop) / (2 * (_a_down - _a_up));
+        step_const = 0;
         step_decel = targSteps - step_accel;
-        step_const = 0;
         f_reach = sqrt(2 * _a_up * step_accel + _f_start * _f_start);
     }
+    else{
+        shortMode = 0;
+        step_accel = (_f_max * _f_max - _f_start * _f_start) / (2 * _a_up);
+        step_decel = (_f_stop * _f_stop - _f_max * _f_max) / (2 * _a_down);
+        step_const = (double)targSteps - step_accel - step_decel;
+    }
     
 }