Akito / trapezoidalMotionCal

台形制御用ライブラリ

Dependents:   kisoken_PenPlotter

Files at this revision

API Documentation at this revision

Revision 2:a43e90cb271c, committed 2017-09-07

Comitter:
Akito914
Date:
Thu Sep 07 06:42:08 2017 +0000
Parent:
1:15adfb7ee64c
Commit message:
???????????????????

Changed in this revision

trapezoidalMotionCal.cpp Show annotated file Show diff for this revision Revisions of this file 
--- 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;
+    }
     
 }