Akito / timeBaseTrapezoidalMotionCal

速度からステップ数を取得する、台形駆動ライブラリ。

timeBaseTrapezoidalMotionCal.cpp@2:ee75bb401e31, 2018-01-07 (annotated)

Committer:
Akito914
Date:
Sun Jan 07 14:28:36 2018 +0000
Revision:
2:ee75bb401e31
????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Akito914 2:ee75bb401e31 1
Akito914 2:ee75bb401e31 2 #include "mbed.h"
Akito914 2:ee75bb401e31 3 #include "timeBaseTrapezoidalMotionCal.h"
Akito914 2:ee75bb401e31 4
Akito914 2:ee75bb401e31 5
Akito914 2:ee75bb401e31 6 timeBaseTrapezoidalMotionCal::timeBaseTrapezoidalMotionCal(double f_start, double f_stop, double f_max, double a_up, double a_down){
Akito914 2:ee75bb401e31 7 _f_start = f_start;
Akito914 2:ee75bb401e31 8 _f_stop = f_stop;
Akito914 2:ee75bb401e31 9 _f_max = f_max;
Akito914 2:ee75bb401e31 10 _a_up = a_up;
Akito914 2:ee75bb401e31 11 _a_down = a_down;
Akito914 2:ee75bb401e31 12
Akito914 2:ee75bb401e31 13 shortMode = 0;
Akito914 2:ee75bb401e31 14 revMode = 0;
Akito914 2:ee75bb401e31 15
Akito914 2:ee75bb401e31 16 }
Akito914 2:ee75bb401e31 17
Akito914 2:ee75bb401e31 18 void timeBaseTrapezoidalMotionCal::setTarg(int32_t targ){
Akito914 2:ee75bb401e31 19
Akito914 2:ee75bb401e31 20 targSteps = targ;
Akito914 2:ee75bb401e31 21
Akito914 2:ee75bb401e31 22 revMode = 0;
Akito914 2:ee75bb401e31 23 if(targSteps < 0){
Akito914 2:ee75bb401e31 24 revMode = 1;
Akito914 2:ee75bb401e31 25 targSteps *= -1;
Akito914 2:ee75bb401e31 26 }
Akito914 2:ee75bb401e31 27
Akito914 2:ee75bb401e31 28 shortMode = 0;
Akito914 2:ee75bb401e31 29 step_accel = (_f_max * _f_max - _f_start * _f_start) / (2 * _a_up);
Akito914 2:ee75bb401e31 30 step_decel = (_f_stop * _f_stop - _f_max * _f_max) / (2 * _a_down);
Akito914 2:ee75bb401e31 31 step_const = (double)targSteps - step_accel - step_decel;
Akito914 2:ee75bb401e31 32 t_accel = (_f_max - _f_start) / _a_up;
Akito914 2:ee75bb401e31 33 t_decel = (_f_stop - _f_max) / _a_down;
Akito914 2:ee75bb401e31 34 t_const = step_const / _f_max;
Akito914 2:ee75bb401e31 35
Akito914 2:ee75bb401e31 36 if (step_const <= 0) {
Akito914 2:ee75bb401e31 37 shortMode = 1;
Akito914 2:ee75bb401e31 38 step_accel = (2 * _a_down * targSteps + _f_start * _f_start - _f_stop * _f_stop) / (2 * (_a_down - _a_up));
Akito914 2:ee75bb401e31 39 step_const = 0;
Akito914 2:ee75bb401e31 40 step_decel = targSteps - step_accel;
Akito914 2:ee75bb401e31 41 f_reach = sqrt(2 * _a_up * step_accel + _f_start * _f_start);
Akito914 2:ee75bb401e31 42 t_accel = ( f_reach - _f_start) / _a_up;
Akito914 2:ee75bb401e31 43 t_decel = (_f_stop - f_reach) / _a_down;
Akito914 2:ee75bb401e31 44 t_const = 0;
Akito914 2:ee75bb401e31 45 }
Akito914 2:ee75bb401e31 46
Akito914 2:ee75bb401e31 47 }
Akito914 2:ee75bb401e31 48
Akito914 2:ee75bb401e31 49 int32_t timeBaseTrapezoidalMotionCal::calSteps(int32_t time_ms){
Akito914 2:ee75bb401e31 50 int32_t steps;
Akito914 2:ee75bb401e31 51
Akito914 2:ee75bb401e31 52 if (time_ms * 0.001 < t_accel) {
Akito914 2:ee75bb401e31 53 steps = _a_up * time_ms * time_ms * 0.000001 / 2.0 + _f_start * time_ms * 0.001;
Akito914 2:ee75bb401e31 54 }
Akito914 2:ee75bb401e31 55 else if (time_ms * 0.001 < t_accel + t_const) {
Akito914 2:ee75bb401e31 56 steps = _f_max * (time_ms * 0.001 - t_accel) + step_accel;
Akito914 2:ee75bb401e31 57 }
Akito914 2:ee75bb401e31 58 else if(time_ms * 0.001 < t_accel + t_const + t_decel){
Akito914 2:ee75bb401e31 59 if (shortMode == 0) {
Akito914 2:ee75bb401e31 60 steps = _a_down * (time_ms * 0.001 - t_accel - t_const) * (time_ms * 0.001 - t_accel - t_const) / 2.0 \
Akito914 2:ee75bb401e31 61 + _f_max * (time_ms * 0.001 - t_accel - t_const) + step_accel + step_const;
Akito914 2:ee75bb401e31 62 }
Akito914 2:ee75bb401e31 63 else {
Akito914 2:ee75bb401e31 64 steps = _a_down * (time_ms * 0.001 - t_accel) * (time_ms * 0.001 - t_accel) / 2.0 \
Akito914 2:ee75bb401e31 65 + f_reach * (time_ms * 0.001 - t_accel) + step_accel;
Akito914 2:ee75bb401e31 66 }
Akito914 2:ee75bb401e31 67 }
Akito914 2:ee75bb401e31 68 else steps = targSteps;
Akito914 2:ee75bb401e31 69
Akito914 2:ee75bb401e31 70 return steps * (revMode ? -1 : 1);
Akito914 2:ee75bb401e31 71 }
Akito914 2:ee75bb401e31 72