速度からステップ数を取得する、台形駆動ライブラリ。
timeBaseTrapezoidalMotionCal.cpp
- Committer:
- Akito914
- Date:
- 2018-01-07
- Revision:
- 2:ee75bb401e31
File content as of revision 2:ee75bb401e31:
#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); }