Diff: timeBaseTrapezoidalMotionCal.cpp

Revision:

2:ee75bb401e31

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timeBaseTrapezoidalMotionCal.cpp	Sun Jan 07 14:28:36 2018 +0000
@@ -0,0 +1,72 @@
+
+#include "mbed.h"
+#include "timeBaseTrapezoidalMotionCal.h"
+
+
+timeBaseTrapezoidalMotionCal::timeBaseTrapezoidalMotionCal(double f_start, double f_stop, double f_max, double a_up, double a_down){
+    _f_start = f_start;
+    _f_stop  = f_stop;
+    _f_max   = f_max;
+    _a_up    = a_up;
+    _a_down  = a_down;
+    
+    shortMode = 0;
+    revMode = 0;
+    
+}
+
+void timeBaseTrapezoidalMotionCal::setTarg(int32_t targ){
+    
+    targSteps = targ;
+    
+    revMode = 0;
+    if(targSteps < 0){
+        revMode = 1;
+        targSteps *= -1;
+    }
+    
+    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;
+    t_accel = (_f_max - _f_start) / _a_up;
+    t_decel = (_f_stop - _f_max) / _a_down;
+    t_const = step_const / _f_max;
+    
+    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;
+        f_reach = sqrt(2 * _a_up * step_accel + _f_start * _f_start);
+        t_accel = ( f_reach - _f_start) / _a_up;
+        t_decel = (_f_stop  -  f_reach) / _a_down;
+        t_const = 0;
+    }
+    
+}
+
+int32_t timeBaseTrapezoidalMotionCal::calSteps(int32_t time_ms){
+    int32_t steps;
+    
+    if (time_ms * 0.001 < t_accel) {
+        steps = _a_up * time_ms * time_ms * 0.000001 / 2.0 + _f_start * time_ms * 0.001;
+    }
+    else if (time_ms * 0.001 < t_accel + t_const) {
+        steps = _f_max * (time_ms * 0.001 - t_accel) + step_accel;
+    }
+    else if(time_ms * 0.001 < t_accel + t_const + t_decel){
+        if (shortMode == 0) {
+            steps = _a_down * (time_ms * 0.001 - t_accel - t_const) * (time_ms * 0.001 - t_accel - t_const) / 2.0      \
+                  + _f_max  * (time_ms * 0.001 - t_accel - t_const) + step_accel + step_const;
+        }
+        else {
+            steps = _a_down * (time_ms * 0.001 - t_accel) * (time_ms * 0.001 - t_accel) / 2.0     \
+                  + f_reach * (time_ms * 0.001 - t_accel) + step_accel;
+        }
+    }
+    else steps = targSteps;
+    
+    return steps * (revMode ? -1 : 1);
+}
+