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/imu.ino

@@ -4,11 +4,12 @@
 
 //#define PRINT_ANGLES
 //#define PRINT_QUAT
-//#define PERFORM_CALIBRATION //Comment to disable startup calibration
+#define PERFORM_CALIBRATION //Comment to disable startup calibration
 
 #define IMU_ADDRESS 0x68    //Change to the address of the IMU
 MPU6050 IMU;               //Change to the name of any supported IMU!
 
+calData calib = {0};
 AccelData IMUAccel;    //Sensor data
 GyroData IMUGyro;
 Madgwick filter;
@@ -18,14 +19,6 @@ void setup_imu(){
   Wire.begin();
   Wire.setClock(400000); //400khz clock
 
-  calData calib;
-  calib.accelBias[0]=-0.02;
-  calib.accelBias[1]=-0.01;
-  calib.accelBias[2]=0.16;
-  calib.gyroBias[0] = -2.89;
-  calib.gyroBias[1] = -3.59;
-  calib.gyroBias[2] = -0.66;
-
   int err = IMU.init(calib, IMU_ADDRESS);
   if (err != 0) {
     Serial.print("Error initializing IMU: ");
@@ -43,9 +36,11 @@ void setup_imu(){
     }
   }
 
+
+
 #ifdef PERFORM_CALIBRATION
   Serial.println("Keep IMU level.");
-  delay(5000);
+  delay(500);
   IMU.calibrateAccelGyro(&calib);
   Serial.println("Calibration done!");
   Serial.println("Accel biases X/Y/Z: ");
@@ -60,8 +55,7 @@ void setup_imu(){
   Serial.print(calib.gyroBias[1]);
   Serial.print(", ");
   Serial.println(calib.gyroBias[2]);
-  delay(5000);
-
+  delay(500);
   IMU.init(calib, IMU_ADDRESS);
 #endif
   filter.begin(0.2f);

selfbalance-madgwick/selfbalance-madgwick.ino

@@ -1,6 +1,4 @@
-#include <ArduPID.h>
-
-ArduPID myController;
+#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,27 +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();
   }
 
-  myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
-  myController.setOutputLimits(-MAX_VELOCITY, MAX_VELOCITY);  // double of max torque motors can exhert
-  //myController.setWindUpLimits(-10, 10);
-  myController.setSampleTime(2);
-  myController.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);
 }
@@ -68,15 +71,35 @@ 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();
-
+  // clean the sign bit
   if(current_time_motors - last_time_motors > halfperiod1){
     // Half a pulse. Next cycle will be the rest
     digitalWriteFast(MOT_DX_STEP, b);
@@ -86,59 +109,28 @@ void loop1(){
   }
 }
 
-unsigned long last_time = millis(), current_time = millis(), time_diff;
 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() {
-  current_time = millis();
-  time_diff = current_time - last_time;
-
   update_imu();
-  // 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(myController.compute()){
-    input = pitch;
 
-    // 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);*/
+  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()){
 
     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);*/
-
-    last_time = current_time;
   }
 
 }