速度からステップ数を取得する、台形駆動ライブラリ。
Revision 2:ee75bb401e31, committed 2018-01-07
- Comitter:
- Akito914
- Date:
- Sun Jan 07 14:28:36 2018 +0000
- Parent:
- 1:15adfb7ee64c
- Commit message:
- ????????
Changed in this revision
--- /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); +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/timeBaseTrapezoidalMotionCal.h Sun Jan 07 14:28:36 2018 +0000 @@ -0,0 +1,45 @@ + +#include "mbed.h" + +#ifndef __TIME_BASE_TRAPEZOIDAL_MOTION_CAL_H__ +#define __TIME_BASE_TRAPEZOIDAL_MOTION_CAL_H__ + + +class timeBaseTrapezoidalMotionCal{ + +public: + + timeBaseTrapezoidalMotionCal(double f_start, double f_stop, double f_max, double a_up, double a_down); + + void setTarg(int32_t targ); + + int32_t calSteps(int32_t time_ms); + +private: + double _f_start; + double _f_stop; + double _f_max; + double _a_up; + double _a_down; + + double step_accel; + double step_decel; + double step_const; + double f_reach; + + double t_accel; + double t_const; + double t_decel; + + int32_t targSteps; + + int shortMode; + int revMode; + +}; + + + + +#endif +
--- a/trapezoidalMotionCal.cpp Thu Aug 03 12:42:12 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,62 +0,0 @@ - -#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; - -} - -void trapezoidalMotionCal::setTarg(int32_t targ){ - - targSteps = targ; - - if(targSteps < 0){ - revMode = 1; - 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_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; -} -
--- a/trapezoidalMotionCal.h Thu Aug 03 12:42:12 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,41 +0,0 @@ - -#include "mbed.h" - -#ifndef __TRAPEZOIDAL_MOTION_CAL_H__ -#define __TRAPEZOIDAL_MOTION_CAL_H__ - - -class trapezoidalMotionCal{ - -public: - - trapezoidalMotionCal(double f_start, double f_stop, double f_max, double a_up, double a_down); - - void setTarg(int32_t targ); - - uint32_t calStepDelay(int32_t steps); - -private: - double _f_start; - double _f_stop; - double _f_max; - double _a_up; - double _a_down; - - double step_accel; - double step_decel; - double step_const; - double f_reach; - - int32_t targSteps; - - int shortMode; - int revMode; - -}; - - - - -#endif -