very simple trapezoidal velocity planner

src/utilities/trapezoids/README.md

@@ -0,0 +1,68 @@
+
+# Trapezoids
+
+Classes for trapezoidal motion profiles. Intended as a starting point for your own higher level control schemes.
+
+## TrapezoidalPlanner
+
+Produces trapzoidal velocity profiles for the motor, used with `MotionControlMode::velocity`.
+
+The following graphs show a commanded angle change from 0 to 10 radians executed by the trapezoidal planner. The accelleration limit is set
+to 1rad/s/s while the decelleration limit is set to 0.25rad/s/s. The maximum velocity is set to 5rad/s, which is not reached, resulting in a triangular shaped
+velocity profile. The position profile is nice smooth shape, as expected.
+
+|Velocity|Position|
+|---|---|
+| ![Velocity Graph](vel_0to10.png) | ![Angle Graph](pos_0to10.png) |
+
+
+In the following example the angle is commanded back from 10 to 0 radians, but this time the velocity limit on the planner is set to 1.25rad/s. You can see the trapezoidal shape of the velocity profile (negative because we're going backwards).
+
+|Velocity|Position|
+|---|---|
+| ![Velocity Graph](vel_10to0.png) | ![Angle Graph](pos_10to0.png) |
+
+
+### Usage
+
+The following *incomplete* code example shows only the parts relevant to integrating the planner:
+
+```c++
+
+Commander commander = Commander(Serial);
+BLDCMotor motor = BLDCMotor(7); // 7 pole pairs
+TrapezoidalPlanner trapezoidal = TrapezoidalPlanner(5.0f, 1.0f, 0.25f, 0.2f);
+
+float target_angle = 0.0f;
+
+void onTarget(char* cmd){ commander.scalar(&target_angle); trapezoidal.setTarget(target_angle); }
+
+void setup() {
+
+    ...
+  motor.controller = MotionControlType::velocity;
+  trapezoidal.linkMotor(motor);
+  motor.init();
+    ...
+  commander.add('T', onTarget, "target angle");
+  trapezoidal.setTarget(target_angle);
+}
+
+
+
+int32_t downsample = 50;    // depending on your MCU's speed, you might want a value between 5 and 50...
+int32_t downsample_cnt = 0;
+
+void loop() {
+
+  if (downsample > 0 && --downsample_cnt <= 0) {
+    motor.target = trapezoidal.run();
+    downsample_cnt = downsample;
+  }
+  motor.move();
+  motor.loopFOC();
+  commander.run();
+
+}
+
+```

src/utilities/trapezoids/TrapezoidalPlanner.cpp

@@ -0,0 +1,52 @@
+
+
+#include "./TrapezoidalPlanner.h"
+
+
+TrapezoidalPlanner::TrapezoidalPlanner(float max, float accel, float decel, float min){
+    this->max_velocity = max;
+    this->accel = accel;
+    this->decel = (decel!=NOT_SET)?decel:accel;
+    this->min_velocity = (min!=NOT_SET)?min:0.2f;
+};
+
+
+TrapezoidalPlanner::~TrapezoidalPlanner(){};
+
+
+
+void TrapezoidalPlanner::linkMotor(FOCMotor& motor){
+    this->motor = &motor;
+};
+
+
+
+void TrapezoidalPlanner::setTarget(float target){
+    this->target = target;
+    this->start_ang = motor->shaft_angle;
+    this->last_target = target;
+};
+
+
+
+float TrapezoidalPlanner::run(){
+    // given the just current position and the target position, calculate the desired velocity
+    // assume we're within our acceleration capabilities
+    float d = target - motor->shaft_angle;
+    float dd = motor->shaft_angle - start_ang;
+    // if the target is reached, return 0
+    if (abs(d) < arrived_distance && abs(motor->shaft_velocity) < arrived_velocity) return 0;
+
+    // how fast can we be going, based on deceleration?
+    float max_v_d = sqrt(2.0f * decel * fabs(d));
+    // how fast can we be going, based on accelleration?
+    float max_v_dd = sqrt(2.0f * accel * fabs(dd));
+    float max_v = min(max_v_d, max_v_dd);
+    // how fast can we be going, based on max speed?
+    max_v = min(max_v, max_velocity);
+    max_v = max(max_v, min_velocity);
+    // let's go that speed :-)
+    return max_v * _sign(d);
+};
+
+

src/utilities/trapezoids/TrapezoidalPlanner.h

@@ -0,0 +1,43 @@
+
+#pragma once
+
+#include "common/base_classes/FOCMotor.h"
+
+/**
+ * Trapezoidal planner
+ * 
+ * Very simple class to demonstrate higher level control on top of the FOC library.
+ * 
+ * This class is used to generate trapezoidal velocity profiles for the motor. 
+ * Configure the motor in MotionControlMode::velocity. Link the motor to the planner,
+ * and set the target angle. The planner will take care of controlling the motor
+ * velocity to follow a trapezoidal profile.
+ * Call the run() method in the loop, and use the return value to update the motor
+ * target velocity. Call the trapezoidal planner less frequently than the motor.move()
+ * method, to ensure the motor has time to reach the target velocity.
+ * 
+ */
+class TrapezoidalPlanner {
+public:
+    TrapezoidalPlanner(float max, float accel, float decel = NOT_SET, float min = NOT_SET);
+    ~TrapezoidalPlanner();
+
+    void linkMotor(FOCMotor& motor);
+    void setTarget(float target);
+    float getTarget() { return target; };
+
+    float run();
+
+    float arrived_distance = 0.02f;
+    float arrived_velocity = 0.2f;
+    float max_velocity;
+    float min_velocity;
+    float accel;
+    float decel;
+
+protected:
+    FOCMotor* motor;
+    float target = NOT_SET;
+    float last_target = NOT_SET;
+    float start_ang = 0.0f;
+};