Akito / trapezoidalMotionCal

台形制御用ライブラリ

Dependents:   kisoken_PenPlotter

trapezoidalMotionCal.cpp@2:a43e90cb271c, 2017-09-07 (annotated)

Committer:
Akito914
Date:
Thu Sep 07 06:42:08 2017 +0000
Revision:
2:a43e90cb271c
Parent:
1:15adfb7ee64c 
???????????????????

Who changed what in which revision?

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