台形制御用ライブラリ
Dependents: kisoken_PenPlotter
Diff: trapezoidalMotionCal.cpp
- Revision:
- 0:a7573f3f2207
- Child:
- 1:15adfb7ee64c
diff -r 000000000000 -r a7573f3f2207 trapezoidalMotionCal.cpp --- /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; +} +