Armdevel: Create PID Library

apps/test-pid/CMakeLists.txt

@@ -1,4 +1,7 @@
 add_executable(test-pid.${TARGET}-board.elf)
 target_sources(test-pid.${TARGET}-board.elf PRIVATE src/main.cpp)
-target_link_libraries(test-pid.${TARGET}-board.elf PRIVATE PID)
+target_link_libraries(test-pid.${TARGET}-board.elf PRIVATE PID SimulatedPIDValues)
 target_set_firmware_properties(test-pid.${TARGET}-board.elf)
+
+add_library(SimulatedPIDValues INTERFACE)
+target_include_directories(SimulatedPIDValues INTERFACE include)

apps/test-pid/src/main.cpp

@@ -1,110 +1,45 @@
-#include "PID.h"
-#include "mbed.h"
-
-// Defines
-#define MIN_RPM              0  // change min/max RPMs based on motor used
-#define MAX_RPM              300
-#define COUNTS_PER_REV       1200   // motor property
-#define TIMER_INTERRUPT_FREQ 250ms  // frequency of timer interrupt for input calculation
-#define GOAL_RPM             100.0
-#define K_UPDATE_PERIOD      150ms
-
-// motor direction pin and pwm out pin, modify as needed
-DigitalOut MOTOR_DIR(D8);
-PwmOut MOTOR_PWM_OUT(D9);
-
-// interrupt in to count encoder rises/falls
-InterruptIn encoderCh1(D2);
-InterruptIn encoderCh2(D3);
-
-PID rpmPIDController(0.010268, 0.0260, 0, K_UPDATE_PERIOD);
-Timer timer;
+/**
+ * This app tests the functionality of our PID library
+ * It uses the setpoint and feedback values defined in setpoint.h and feedback.h respectively
+ * Next, the control signal generated by our PID library is compared to that in control.h
+ * The data found in the header files was generated using a MatLab script
+ * NOTE: This test-app is purely computational, no motors or encoders are required
+ */
 
-// Variables
-uint64_t pulseCount    = 0;
-uint64_t oldPulseCount = 0;
-float motorRPM         = 0.0;
-float motorPWMDuty     = 0.0;
-Ticker interruptTimer;
+#include <numeric>
 
-// PID AutoTune config struct for specific DC motor, change depending on actuator
-PID::t_AutoTuneConfig autoTuneConfig = {
-    .nLookBack = 40, .sampleTime = 250ms, .outputStart = 0.4, .oStep = 0.25, .noiseBand = 0.01, .setpoint = 120};
+#include "PID.h"
+#include "control.h"
+#include "feedback.h"
+#include "setpoint.h"
 
-// Setup velocity PID controller
-void initializePidController(void) {
-  rpmPIDController.setInputLimits(MIN_RPM, MAX_RPM);
-  rpmPIDController.setOutputLimits(0.0, 1.0);
-  rpmPIDController.setBias(0.0);
-  rpmPIDController.setMode(PID_AUTO_MODE);
-  rpmPIDController.setupAutoTune(&MOTOR_PWM_OUT, &motorRPM, 0);
-}
+constexpr uint8_t KP      = 2;
+constexpr uint8_t KI      = 103;
+constexpr uint8_t KD      = 1;
+constexpr int32_t min_rpm = INT32_MIN, max_rpm = INT32_MAX;  // no saturation
+constexpr float deadzone  = 0;
+constexpr auto pid_period = 1ms;
 
-// every time a pulse is received from the encoder channels, increase the pulse count
-void countPulses() {
-  pulseCount++;
-}
-
-// every timer interrupt, recompute the rpm
-void computeInput() {
-  motorRPM = (pulseCount - oldPulseCount) /
-             std::chrono::duration_cast<std::chrono::duration<float, std::ratio<60>>>(TIMER_INTERRUPT_FREQ).count() /
-             COUNTS_PER_REV;
-  oldPulseCount = pulseCount;
-}
+std::array<float, 5001> control_error;  // compare matlab's control signal to ours
 
 int main() {
-  encoderCh1.rise(&countPulses);  // attach the address of the pulse count function to the rising edge
-  encoderCh1.fall(&countPulses);
-  encoderCh2.rise(&countPulses);
-  encoderCh2.fall(&countPulses);
-
-  printf("PID Test - Start \r\n");
-
-  // Initialization
-  std::chrono::duration<float> interval = 0.1s;
-  initializePidController();
-  rpmPIDController.setSetPoint(GOAL_RPM);                      // Set RPM set point
-  MOTOR_DIR = 1;                                               // set default direction
-  interruptTimer.attach(&computeInput, TIMER_INTERRUPT_FREQ);  // attach function to timer interrupt
-  timer.start();
-
-  // uncomment below line and comment line after for debug output
-  // rpmPIDController.autoTune(&pc, true, &autoTuneConfig);
-  rpmPIDController.autoTune(true, &autoTuneConfig);
-
-  printf("Autotune Params obtained: Kc: %f \t    TauI: %f \t    TauD: %f \r\n", rpmPIDController.getATunePParam(),
-         rpmPIDController.getATuneIParam(), rpmPIDController.getATuneDParam());
-  rpmPIDController.setAutoTuneParams();
-  interruptTimer.detach();
-
-  ThisThread::sleep_for(5s);
-
-  Timer eval;
-  eval.start();
-
-  while (1) {
-    motorRPM = (pulseCount - oldPulseCount) *
-               std::chrono::duration_cast<std::chrono::duration<float, std::ratio<60>>>(interval).count() /
-               COUNTS_PER_REV;
-    oldPulseCount = pulseCount;
-
-    // Update the PID controller
-    rpmPIDController.setInterval(interval);
-    rpmPIDController.setProcessValue(motorRPM);
-    motorPWMDuty  = rpmPIDController.compute();
-    MOTOR_PWM_OUT = motorPWMDuty;
-
-    printf("Motor RPM: %f, \t Goal RPM: %f, \t PWM Output: %f\r\n", motorRPM, GOAL_RPM, motorPWMDuty);
-    if (abs(motorRPM - GOAL_RPM) < 1.0) {
-      printf("Time taken to reach goal RPM: %f seconds \r\n",
-             std::chrono::duration_cast<std::chrono::duration<float>>(eval.elapsed_time()).count());
-      MOTOR_DIR = 0;
-      return 0;
+  printf("##################### PID TEST APP STARTED #####################\r\n");
+  PID::Pid controller(KP, KI, KD, min_rpm, max_rpm, deadzone, false);
+  Timer timer;
+  for (uint64_t i = 0; i < control_error.size(); i++) {
+    if (i % 1000 == 0) {
+      printf("Completed %lld/50001 iterations\r\n", (long long)i);
     }
-
-    ThisThread::sleep_for(K_UPDATE_PERIOD);
-    interval = std::chrono::duration_cast<std::chrono::duration<float>>(timer.elapsed_time());
     timer.reset();
+    timer.start();
+    control_error[i] = std::abs(control.at(i) - controller.compute(setpoint.at(i), feedback.at(i)));
+    timer.stop();
+    MBED_ASSERT(pid_period > timer.elapsed_time());
+    wait_us((pid_period - timer.elapsed_time()).count());  // account for compute time
   }
+  printf("ERROR STATISTICS\r\n");
+  float average = std::accumulate(control_error.begin(), control_error.end(), 0.0) / control_error.size();
+  printf("Average difference between Matlab control signal and our control signal: %.3f\r\n", average);
+  while (true)
+    ;
 }

lib/controllers/include/ActuatorController.h

@@ -19,7 +19,7 @@ class ActuatorController {
     float minVelocity_DegreesPerSec = -10.0, maxVelocity_DegreesPerSec = +10.0;
     float minAngle_Degrees = -90, maxAngle_Degrees = +90;
 
-    PID::t_pidConfig velocityPID, positionPID;
+    PID::Config velocityPID, positionPID;
 
     std::chrono::duration<float> watchDogTimeout = 3.0s;
   } t_actuatorConfig;
@@ -62,8 +62,8 @@ class ActuatorController {
   bool m_limSwitchMin_Connected;
   bool m_limSwitchMax_Connected;
 
-  PID m_velocityPIDController;
-  PID m_positionPIDController;
+  PID::Pid m_velocityPIDController;
+  PID::Pid m_positionPIDController;
 
   Timer m_updateTimer;
 

lib/controllers/src/ActuatorController.cpp

@@ -11,10 +11,8 @@ ActuatorController::ActuatorController(t_actuatorConfig actuatorConfig, Motor &m
       r_encoder(encoder),
       r_limSwitchMin(limSwitchMin),
       r_limSwitchMax(limSwitchMax),
-      m_velocityPIDController(actuatorConfig.velocityPID.P, actuatorConfig.velocityPID.I, actuatorConfig.velocityPID.D,
-                              0.0s),
-      m_positionPIDController(actuatorConfig.positionPID.P, actuatorConfig.positionPID.I, actuatorConfig.positionPID.D,
-                              0.0s) {
+      m_velocityPIDController(actuatorConfig.velocityPID),
+      m_positionPIDController(actuatorConfig.positionPID) {
   m_limSwitchMin_Connected = (r_limSwitchMin != NULL_DIGITAL_IN && r_limSwitchMin.is_connected());
   m_limSwitchMax_Connected = (r_limSwitchMax != NULL_DIGITAL_IN && r_limSwitchMax.is_connected());