Implement Robust PID and Prepare for removing ARM DSP dependency

examples/motor/m3508.cc

@@ -18,6 +18,8 @@
  *                                                                          *
  ****************************************************************************/
 
+#include <memory>
+
 #include "bsp_gpio.h"
 #include "bsp_print.h"
 #include "cmsis_os.h"
@@ -27,28 +29,25 @@
 #define KEY_GPIO_GROUP GPIOB
 #define KEY_GPIO_PIN GPIO_PIN_2
 
-bsp::CAN* can1 = NULL;
-control::MotorCANBase* motor = NULL;
+void RM_RTOS_Default_Task(const void* args) {
+  UNUSED(args);
 
-void RM_RTOS_Init() {
   print_use_uart(&huart8);
 
-  can1 = new bsp::CAN(&hcan1, 0x201);
-  motor = new control::Motor3508(can1, 0x201);
-}
+  auto can1 = std::make_shared<bsp::CAN>(&hcan1, 0x201);
+  auto motor = std::make_unique<control::Motor3508>(can1, 0x201);
+  auto key = std::make_unique<bsp::GPIO>(KEY_GPIO_GROUP, GPIO_PIN_2);
 
-void RM_RTOS_Default_Task(const void* args) {
-  UNUSED(args);
-  control::MotorCANBase* motors[] = {motor};
+  const std::vector<control::MotorCANBase*> motors = {motor.get()};
 
-  bsp::GPIO key(KEY_GPIO_GROUP, GPIO_PIN_2);
   while (1) {
-    motor->PrintData();
-    if (key.Read())
+    const uint32_t start = osKernelSysTick();
+    if (key->Read())
       motor->SetOutput(800);
     else
       motor->SetOutput(0);
-    control::MotorCANBase::TransmitOutput(motors, 1);
-    osDelay(100);
+    control::MotorCANBase::TransmitOutput(motors);
+    motor->PrintData();
+    osDelayUntil(start + 10);
   }
 }

examples/motor/m3508_speed.cc

@@ -29,35 +29,27 @@
 #define KEY_GPIO_PIN GPIO_PIN_2
 
 #define TARGET_SPEED 80
-
-bsp::CAN* can1 = NULL;
-control::MotorCANBase* motor = NULL;
-
-void RM_RTOS_Init() {
-  print_use_uart(&huart8);
-
-  can1 = new bsp::CAN(&hcan1, 0x201);
-  motor = new control::Motor3508(can1, 0x201);
-}
+#define CONTROL_DT 10
 
 void RM_RTOS_Default_Task(const void* args) {
   UNUSED(args);
-  control::MotorCANBase* motors[] = {motor};
-  control::PIDController pid(20, 8, 0);
 
-  bsp::GPIO key(KEY_GPIO_GROUP, GPIO_PIN_2);
+  print_use_uart(&huart8);
+
+  auto can1 = std::make_shared<bsp::CAN>(&hcan1, 0x201);
+  auto motor = std::make_unique<control::Motor3508>(can1, 0x201);
+  auto pid = std::make_unique<control::PIDController>(20, 0.01, 1, CONTROL_DT / 1e3, 20);
+  auto key = std::make_unique<bsp::GPIO>(KEY_GPIO_GROUP, KEY_GPIO_PIN);
 
-  float target;
+  const std::vector<control::MotorCANBase*> motors = {motor.get()};
 
   while (1) {
-    if (key.Read())
-      target = TARGET_SPEED;
-    else
-      target = 0;
+    const uint32_t start = osKernelSysTick();
+    const float target = key->Read() ? TARGET_SPEED : 0;
 
-    motor->SetOutput(pid.ComputeOutput(motor->GetOmegaDelta(target)));
-    control::MotorCANBase::TransmitOutput(motors, 1);
+    motor->SetOutput(pid->ComputeOutput(motor->GetOmegaDelta(target)));
+    control::MotorCANBase::TransmitOutput(motors);
     motor->PrintData();
-    osDelay(10);
+    osDelayUntil(start + CONTROL_DT);
   }
 }

examples/motor/m6623.cc

@@ -18,6 +18,8 @@
  *                                                                          *
  ****************************************************************************/
 
+#include <memory>
+
 #include "bsp_gpio.h"
 #include "bsp_print.h"
 #include "cmsis_os.h"
@@ -27,28 +29,27 @@
 #define KEY_GPIO_GROUP GPIOB
 #define KEY_GPIO_PIN GPIO_PIN_2
 
-bsp::CAN* can1 = NULL;
-control::MotorCANBase* motor = NULL;
+void RM_RTOS_Default_Task(const void* args) {
+  UNUSED(args);
 
-void RM_RTOS_Init() {
   print_use_uart(&huart8);
 
-  can1 = new bsp::CAN(&hcan1, 0x201);
-  motor = new control::Motor6623(can1, 0x209);
-}
+  auto can1 = std::make_shared<bsp::CAN>(&hcan1, 0x201);
+  auto motor = std::make_unique<control::Motor6623>(can1, 0x209);
+  auto key = std::make_unique<bsp::GPIO>(KEY_GPIO_GROUP, KEY_GPIO_PIN);
 
-void RM_RTOS_Default_Task(const void* args) {
-  UNUSED(args);
-  control::MotorCANBase* motors[] = {motor};
+  const std::vector<control::MotorCANBase*> motors = {motor.get()};
 
-  bsp::GPIO key(KEY_GPIO_GROUP, GPIO_PIN_2);
   while (1) {
-    motor->PrintData();
-    if (key.Read())
+    const uint32_t start = osKernelSysTick();
+
+    if (key->Read())
       motor->SetOutput(400);
     else
       motor->SetOutput(0);
-    control::MotorCANBase::TransmitOutput(motors, 1);
-    osDelay(100);
+
+    control::MotorCANBase::TransmitOutput(motors);
+    motor->PrintData();
+    osDelayUntil(start + 10);
   }
 }

shared/bsp/bsp_memory.cc

@@ -18,6 +18,7 @@
  *                                                                          *
  ****************************************************************************/
 
+#include <cassert>
 #include <cstddef>
 
 #include "cmsis_os.h"

shared/libraries/controller.cc

@@ -2,13 +2,25 @@
 
 namespace control {
 
-PIDController::PIDController(float kp, float ki, float kd) {
-  pid_f32_.Kp = kp;
-  pid_f32_.Ki = ki;
-  pid_f32_.Kd = kd;
-  arm_pid_init_f32(&pid_f32_, 1);
-}
+PIDController::PIDController(float kp, float ki, float kd, float N, float dt)
+    : a0_(1 + N * dt),
+      uc1_((2 + N * dt) / a0_),
+      uc2_(-1 / a0_),
+      ec0_(kp + (ki * dt * a0_ + kd * N) / a0_),
+      ec1_(-(kp * (2 + N * dt) + ki * dt + 2 * kd * N) / a0_),
+      ec2_((kp + kd * N) / a0_) {}
+
+float PIDController::ComputeOutput(float error) {
+  // compute output
+  float u0 = uc1_ * u1_ + uc2_ * u2_ + ec0_ * error + ec1_ * e1_ + ec2_ * e2_;
 
-float PIDController::ComputeOutput(float error) { return arm_pid_f32(&pid_f32_, error); }
+  // update state
+  e2_ = e1_;
+  e1_ = error;
+  u2_ = u1_;
+  u1_ = u0;
+
+  return u0;
+}
 
 } /* namespace control */

shared/libraries/controller.h

@@ -1,17 +1,11 @@
 #pragma once
 
-/* NOTE(alvin): DSP libraries depends on macro definitions on FPU computability, so the
- *              main.h must be included before arm_math.h */
-// clang-format off
 #include "main.h"
-// clang-format on
-
-#include "arm_math.h"
 
 namespace control {
 
 /**
- * @brief simple PID controller
+ * @brief simple PID controller with lowpass filtered derivitive estimates
  */
 class PIDController {
  public:
@@ -21,8 +15,10 @@ class PIDController {
    * @param kp proportional gain
    * @param ki integral gain
    * @param kd derivative gain
+   * @param N  derivative lowpass filter bandwidth in [rad / s]
+   * @param dt sampling time for the control loop in [s]
    */
-  PIDController(float kp, float ki, float kd);
+  PIDController(float kp, float ki, float kd, float N, float dt);
 
   /**
    * @brief compute output base on current error
@@ -34,7 +30,19 @@ class PIDController {
   float ComputeOutput(float error);
 
  private:
-  arm_pid_instance_f32 pid_f32_;
+  // states
+  float u1_ = 0;
+  float u2_ = 0;
+  float e1_ = 0;
+  float e2_ = 0;
+
+  // coefficients
+  const float a0_;
+  const float uc1_;
+  const float uc2_;
+  const float ec0_;
+  const float ec1_;
+  const float ec2_;
 };
 
 } /* namespace control */

shared/libraries/motor.cc

@@ -20,7 +20,8 @@
 
 #include "motor.h"
 
-#include "arm_math.h"
+#include <cmath>
+
 #include "bsp_error_handler.h"
 #include "utils.h"
 
@@ -33,7 +34,7 @@ static void can_motor_callback(const uint8_t data[], void* args) {
   motor->UpdateData(data);
 }
 
-MotorCANBase::MotorCANBase(bsp::CAN* can, uint16_t rx_id)
+MotorCANBase::MotorCANBase(const std::shared_ptr<bsp::CAN>& can, uint16_t rx_id)
     : theta_(0), omega_(0), can_(can), rx_id_(rx_id) {
   constexpr uint16_t GROUP_SIZE = 4;
   constexpr uint16_t RX1_ID_START = 0x201;
@@ -51,14 +52,16 @@ MotorCANBase::MotorCANBase(bsp::CAN* can, uint16_t rx_id)
     tx_id_ = TX2_ID;
   else
     tx_id_ = TX1_ID;
+
+  can->RegisterRxCallback(rx_id, can_motor_callback, this);
 }
 
-void MotorCANBase::TransmitOutput(MotorCANBase* motors[], uint8_t num_motors) {
+void MotorCANBase::TransmitOutput(const std::vector<MotorCANBase*>& motors) {
   uint8_t data[8] = {0};
 
-  RM_ASSERT_GT(num_motors, 0, "Meaningless empty can motor transmission");
-  RM_ASSERT_LT(num_motors, 4, "Exceeding maximum of 4 motor commands per CAN message");
-  for (uint8_t i = 0; i < num_motors; ++i) {
+  RM_ASSERT_GT(motors.size(), 0, "Meaningless empty can motor transmission");
+  RM_ASSERT_LT(motors.size(), 4, "Exceeding maximum of 4 motor commands per CAN message");
+  for (uint8_t i = 0; i < motors.size(); ++i) {
     RM_ASSERT_EQ(motors[i]->tx_id_, motors[0]->tx_id_, "tx id mismatch");
     RM_ASSERT_EQ(motors[i]->can_, motors[0]->can_, "can line mismatch");
     const uint8_t motor_idx = (motors[i]->rx_id_ - 1) % 4;
@@ -73,25 +76,21 @@ void MotorCANBase::TransmitOutput(MotorCANBase* motors[], uint8_t num_motors) {
 float MotorCANBase::GetTheta() const { return theta_; }
 
 float MotorCANBase::GetThetaDelta(float target) const {
-  return wrap<float>(target - theta_, -PI, PI);
+  return wrap<float>(target - theta_, -M_PI, M_PI);
 }
 
 float MotorCANBase::GetOmega() const { return omega_; }
 
 float MotorCANBase::GetOmegaDelta(float target) const { return target - omega_; }
 
-Motor3508::Motor3508(CAN* can, uint16_t rx_id) : MotorCANBase(can, rx_id) {
-  can->RegisterRxCallback(rx_id, can_motor_callback, this);
-}
-
 void Motor3508::UpdateData(const uint8_t data[]) {
   const int16_t raw_theta = data[0] << 8 | data[1];
   const int16_t raw_omega = data[2] << 8 | data[3];
   raw_current_get_ = data[4] << 8 | data[5];
   raw_temperature_ = data[6];
 
-  constexpr float THETA_SCALE = 2 * PI / 8192;  // digital -> rad
-  constexpr float OMEGA_SCALE = 2 * PI / 60;    // rpm -> rad / sec
+  constexpr float THETA_SCALE = 2 * M_PI / 8192;  // digital -> rad
+  constexpr float OMEGA_SCALE = 2 * M_PI / 60;    // rpm -> rad / sec
   theta_ = raw_theta * THETA_SCALE;
   omega_ = raw_omega * OMEGA_SCALE;
 }
@@ -108,16 +107,12 @@ void Motor3508::SetOutput(int16_t val) {
   output_ = clip<int16_t>(val, -MAX_ABS_CURRENT, MAX_ABS_CURRENT);
 }
 
-Motor6623::Motor6623(CAN* can, uint16_t rx_id) : MotorCANBase(can, rx_id) {
-  can->RegisterRxCallback(rx_id, can_motor_callback, this);
-}
-
 void Motor6623::UpdateData(const uint8_t data[]) {
   const int16_t raw_theta = data[0] << 8 | data[1];
   raw_current_get_ = (data[2] << 8 | data[3]) * CURRENT_CORRECTION;
   raw_current_set_ = (data[4] << 8 | data[5]) * CURRENT_CORRECTION;
 
-  constexpr float THETA_SCALE = 2 * PI / 8192;
+  constexpr float THETA_SCALE = 2 * M_PI / 8192;
   theta_ = raw_theta * THETA_SCALE;
 }
 
@@ -143,17 +138,13 @@ float Motor6623::GetOmegaDelta(const float target) const {
   return 0;
 }
 
-Motor2006::Motor2006(CAN* can, uint16_t rx_id) : MotorCANBase(can, rx_id) {
-  can->RegisterRxCallback(rx_id, can_motor_callback, this);
-}
-
 void Motor2006::UpdateData(const uint8_t data[]) {
   const int16_t raw_theta = data[0] << 8 | data[1];
   const int16_t raw_omega = data[2] << 8 | data[3];
   raw_current_get_ = data[4] << 8 | data[5];
 
-  constexpr float THETA_SCALE = 2 * PI / 8192;  // digital -> rad
-  constexpr float OMEGA_SCALE = 2 * PI / 60;    // rpm -> rad / sec
+  constexpr float THETA_SCALE = 2 * M_PI / 8192;  // digital -> rad
+  constexpr float OMEGA_SCALE = 2 * M_PI / 60;    // rpm -> rad / sec
   theta_ = raw_theta * THETA_SCALE;
   omega_ = raw_omega * OMEGA_SCALE;
 }