ArduPID -> QuickPID library

basically the same library, plus the modifications I made myself

plus let the second core also handle the direction of the motors

selfbalance-madgwick/selfbalance-madgwick.ino

@@ -1,6 +1,4 @@
-#include <ArduPID.h>
-
-ArduPID pitchCtrl;
+#include <QuickPID.h>
 
 #define MOT_DX_STEP 19
 #define MOT_DX_DIR 18
@@ -20,18 +18,22 @@ constexpr double MAX_VELOCITY = 1000000 * WHEEL_RADIUS / (2 * MAX_HALF_PERIOD *
 // Derived and analytical model, linearized it and simulated in MATLAB.
 // PID values are then calculated and verified by simulation in Simulink. I ain't calibrating a PID by hand on this robot
 // I modified the ArduPID library to make it accept negative values for the parameters
-constexpr double KP = -50;
-constexpr double KI = -600;
-constexpr double KD = 0.05; 
+constexpr double KP = 42;
+constexpr double KI = KP * 10.8852;
+constexpr double KD = 0.3; 
 
 // IMU little bit tilted
 // TODO: Implement an outer control loop for angular velocity. But that requires encoders on the motors
 // TODO: try to achieve it crudely by just using a PI controller on the velocity given by the PID balance controller
-double setpoint = -0.06;
-double output = 0;
-double input = 0;
+float setpoint = 0.0;
+float output = 0;
+float input = 0;
 
-double yaw{ 0 }, pitch{ 0 }, roll{ 0 };
+float yaw{ 0 }, pitch{ 0 }, roll{ 0 };
+
+
+
+QuickPID pitchCtrl(&input, &output, &setpoint, KP, KI, KD, pitchCtrl.pMode::pOnError, pitchCtrl.dMode::dOnMeas, pitchCtrl.iAwMode::iAwCondition, pitchCtrl.Action::reverse);
 
 void setup() {
   Serial.begin(9600);
@@ -40,29 +42,28 @@ void setup() {
 
   setup_imu();
   
-  pinMode(MOT_DX_DIR, OUTPUT);
-  pinMode(MOT_SX_DIR, OUTPUT); 
-
   // Just to signal it is working
   pinMode(LED_BUILTIN, OUTPUT);
   digitalWrite(LED_BUILTIN, HIGH);
 
   // Let the initial error from madgwick filter discharge without affecting the integral term of the PID
   unsigned long t = millis();
-  while (millis() - t < 2000) {
+  while (millis() - t < 5000) {
     update_imu();
   }
 
-  pitchCtrl.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
-  pitchCtrl.setOutputLimits(-MAX_VELOCITY, MAX_VELOCITY); 
-  //pitchCtrl.setWindUpLimits(-10, 10);
-  pitchCtrl.setSampleTime(1);
-  pitchCtrl.start();
+  // Backward because all coefficients need to be negative
+  pitchCtrl.SetOutputLimits(-MAX_VELOCITY, MAX_VELOCITY);
+  pitchCtrl.SetMode(pitchCtrl.Control::automatic);
+  pitchCtrl.SetSampleTimeUs(1000);
 
   digitalWrite(LED_BUILTIN, LOW);
 }
 
 void setup1(){
+  pinMode(MOT_DX_DIR, OUTPUT);
+  pinMode(MOT_SX_DIR, OUTPUT); 
+
   pinMode(MOT_DX_STEP, OUTPUT);
   pinMode(MOT_SX_STEP, OUTPUT);
 }
@@ -70,15 +71,34 @@ void setup1(){
 unsigned long last_time_motors = micros(), current_time_motors = micros();
 bool b = true;
 // Such a long halfperiod means that the motors are not moving
-uint32_t halfperiod1 = INT_MAX;
+uint32_t t = INT_MAX;
+int32_t period = INT_MAX, halfperiod1 = INT_MAX;
+
 void loop1(){
   // TODO: handle the steppers using interrupt timers. The second core could be used for IMU processing, while the first one handles the different control loops
 
   // Retrieve the half period value from core0. Non blocking call
-  if(rp2040.fifo.available()) rp2040.fifo.pop_nb(&halfperiod1);
+  if(rp2040.fifo.available()) {
+    rp2040.fifo.pop_nb(&t);
+    
+    int32_t period = (int32_t)t;
+
+    // Direction need to be changed during motor pulse
+    // Doing it here unsure it happens at the correct time
+    if(period > 0){
+      // Positivie direction
+      digitalWriteFast(MOT_DX_DIR, HIGH);
+      digitalWriteFast(MOT_SX_DIR, HIGH);
+      halfperiod1 = (uint32_t)(period);
+    }else{
+      // Negative direction
+      digitalWriteFast(MOT_DX_DIR, LOW);
+      digitalWriteFast(MOT_SX_DIR, LOW);
+      halfperiod1 = (uint32_t)(-period);
+    }
+  }
 
   current_time_motors = micros();
-
   if(current_time_motors - last_time_motors > halfperiod1){
     // Half a pulse. Next cycle will be the rest
     digitalWriteFast(MOT_DX_STEP, b);
@@ -89,54 +109,27 @@ void loop1(){
 }
 
 double frequency = 0;
-unsigned long half_period0 = 0;
+int32_t half_period0 = 0;
 double velocity = 0;
 
+uint32_t current_time = millis(), last_time = millis();
+
 void loop() {
   update_imu();
 
   input = pitch;
+  //if(abs(input -setpoint) <= 0.01) input = setpoint;
   
   // I also modified the ArduPID library to use compute as a boolean. If calculations were done, it returns true. If not enough time has elapsed, it returns false
-  if(pitchCtrl.compute()){
-
-    // Keeping it here
-    /*
-    double force_per_motor = output / WHEEL_RADIUS;
-    double accel = force_per_motor / (WEIGHT);
-    velocity += accel * time_diff * 0.001;
-    // anti-windup
-    velocity = constrain(velocity, -MAX_VELOCITY, MAX_VELOCITY);*/
+  if(pitchCtrl.Compute()){
 
     double tvelocity = output;
-    if(tvelocity < 0){
-      digitalWriteFast(MOT_DX_DIR, LOW);
-      digitalWriteFast(MOT_SX_DIR, LOW);
-      tvelocity = -tvelocity;
-    }else{
-      digitalWriteFast(MOT_DX_DIR, HIGH);
-      digitalWriteFast(MOT_SX_DIR, HIGH);
-    }
-
-    tvelocity = tvelocity * 180 / PI;
-    frequency = tvelocity *  0.1125 / WHEEL_RADIUS;
+    tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
+    frequency = tvelocity *  0.1125;
     half_period0 = 1000000 / (2*frequency);
 
     // Send the half period to core 1. Non blocking
     rp2040.fifo.push_nb(half_period0);
-
-    // Some ugly logging I used in debugging, keeping in around
-    /*Serial.println(input);
-    Serial.print(" | ");
-    Serial.print(output);
-    Serial.print(" | ");
-    Serial.print(accel);
-    Serial.print(" | ");
-    Serial.print(velocity);
-    Serial.print(" | ");
-    Serial.print(frequency);
-    Serial.print(" | ");
-    Serial.println(half_period0);*/
   }
 
 }