Diff: trapezoidalMotionCal.cpp

Revision:

0:a7573f3f2207

Child:

1:15adfb7ee64c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trapezoidalMotionCal.cpp	Thu Aug 03 06:50:03 2017 +0000
@@ -0,0 +1,64 @@
+
+#include "mbed.h"
+#include "trapezoidalMotionCal.h"
+
+
+trapezoidalMotionCal::trapezoidalMotionCal(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;
+    
+    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);
+    
+    
+}
+
+void trapezoidalMotionCal::setTarg(int32_t targ){
+    
+    targSteps = targ;
+    
+    if(targSteps < 0){
+        revMode = 1;
+        targSteps *= -1;
+    }
+    
+    step_const = (double)targSteps - step_accel - step_decel;
+    
+    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_decel = targSteps - step_accel;
+        step_const = 0;
+        f_reach = sqrt(2 * _a_up * step_accel + _f_start * _f_start);
+    }
+    
+}
+
+uint32_t trapezoidalMotionCal::calStepDelay(int32_t steps){
+    uint32_t period_us;
+    
+    if (steps < step_accel) {
+        period_us = 1000000 * pow(_f_start * _f_start + 2 * _a_up * steps, -0.5);
+    }
+    else if (steps < step_accel + step_const) {
+        period_us = 1000000 / _f_max;
+    }
+    else {
+        if (shortMode == 0) {
+            period_us = 1000000 * pow(_f_max * _f_max + 2 * _a_down * (steps - step_accel - step_const), -0.5);
+        }
+        else {
+            period_us = 1000000 * pow(f_reach * f_reach + 2 * _a_down * (steps - step_accel - step_const), -0.5);
+        }
+        
+    }
+    
+    return period_us;
+}
+