Akito
速度からステップ数を取得する、台形駆動ライブラリ。
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
diff -r 15adfb7ee64c -r ee75bb401e31 timeBaseTrapezoidalMotionCal.cpp
--- /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);
+}
+
diff -r 15adfb7ee64c -r ee75bb401e31 timeBaseTrapezoidalMotionCal.h
--- /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
+
diff -r 15adfb7ee64c -r ee75bb401e31 trapezoidalMotionCal.cpp
--- 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;
-}
-
diff -r 15adfb7ee64c -r ee75bb401e31 trapezoidalMotionCal.h
--- 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
-