initial yaw ctrl for turning

gyro drift hits hard

selfbalance-madgwick/imu.ino

@@ -4,7 +4,7 @@
 
 //#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!
@@ -36,26 +36,10 @@ void setup_imu(){
     }
   }
 
-
-
 #ifdef PERFORM_CALIBRATION
-  Serial.println("Keep IMU level.");
-  delay(500);
+  delay(1500);
   IMU.calibrateAccelGyro(&calib);
-  Serial.println("Calibration done!");
-  Serial.println("Accel biases X/Y/Z: ");
-  Serial.print(calib.accelBias[0]);
-  Serial.print(", ");
-  Serial.print(calib.accelBias[1]);
-  Serial.print(", ");
-  Serial.println(calib.accelBias[2]);
-  Serial.println("Gyro biases X/Y/Z: ");
-  Serial.print(calib.gyroBias[0]);
-  Serial.print(", ");
-  Serial.print(calib.gyroBias[1]);
-  Serial.print(", ");
-  Serial.println(calib.gyroBias[2]);
-  delay(500);
+  delay(1500);
   IMU.init(calib, IMU_ADDRESS);
 #endif
   filter.begin(0.2f);

selfbalance-madgwick/selfbalance-madgwick.ino

@@ -15,25 +15,45 @@ constexpr double MAX_HALF_PERIOD = 75; // in microseconds
 // Which means there is a maximum velocity achievable
 constexpr double MAX_VELOCITY = 1000000 * WHEEL_RADIUS / (2 * MAX_HALF_PERIOD * ANGLE_PER_STEP) * PI / 180;
 
+constexpr double CONVERSION_FACTOR =  180 / WHEEL_RADIUS / PI;
+
 // 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 = 42;
-constexpr double KI = KP * 10.8852;
-constexpr double KD = 0.3; 
+constexpr float KP = 42;
+constexpr float KI = KP * 10.8852;
+constexpr float KD = 0.3;
+
+// PI constants for outer velocity control loop
+constexpr float KP_vel = 0.0065;
+constexpr float KI_vel = 0.0005;
+
+constexpr float KP_yaw = 0.8;
+constexpr float KI_yaw = 0.06;
+
+float angle_setpoint = 0.0;
+float angle_output = 0;
+float angle_input = 0;
+
+float yaw_setpoint = 0.0;
+float yaw_output = 0;
+float yaw_input = 0;
+
+float vel_setpoint = 0.0;
+float vel_input = 0.0;
+
 
-// 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
-float setpoint = 0.0;
-float output = 0;
-float input = 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);
+QuickPID pitchCtrl(&pitch, &angle_output, &angle_setpoint, KP, KI, KD, pitchCtrl.pMode::pOnError, pitchCtrl.dMode::dOnMeas, pitchCtrl.iAwMode::iAwCondition, pitchCtrl.Action::reverse);
+// TODO: a little deadzone when the input is almost 0, just to avoid unnecessary "nervous" control and eventual oscillations
+QuickPID velCtrl(&angle_output, &angle_setpoint, &vel_setpoint, KP_vel, KI_vel, 0, velCtrl.pMode::pOnError, velCtrl.dMode::dOnMeas, velCtrl.iAwMode::iAwClamp, velCtrl.Action::direct);
+
+QuickPID yawCtrl(&yaw, &yaw_output, &yaw_setpoint, KP_yaw, KI_yaw, 0, yawCtrl.pMode::pOnError, yawCtrl.dMode::dOnMeas, yawCtrl.iAwMode::iAwClamp, yawCtrl.Action::direct);
+
 
 void setup() {
   Serial.begin(9600);
@@ -48,7 +68,7 @@ void setup() {
 
   // Let the initial error from madgwick filter discharge without affecting the integral term of the PID
   unsigned long t = millis();
-  while (millis() - t < 5000) {
+  while (millis() - t < 2000) {
     update_imu();
   }
 
@@ -57,10 +77,20 @@ void setup() {
   pitchCtrl.SetMode(pitchCtrl.Control::automatic);
   pitchCtrl.SetSampleTimeUs(1000);
 
+  velCtrl.SetOutputLimits(-0.35, 0.35);
+  velCtrl.SetMode(velCtrl.Control::automatic);
+  velCtrl.SetSampleTimeUs(10000);
+
+  yawCtrl.SetOutputLimits(-MAX_VELOCITY*0.01, MAX_VELOCITY*0.01);
+  yawCtrl.SetMode(yawCtrl.Control::automatic);
+  yawCtrl.SetSampleTimeUs(10000);
+
   digitalWrite(LED_BUILTIN, LOW);
+
+  yaw_setpoint = yaw;
 }
 
-void setup1(){
+void setup1() {
   pinMode(MOT_DX_DIR, OUTPUT);
   pinMode(MOT_SX_DIR, OUTPUT);
 
@@ -68,43 +98,53 @@ void setup1(){
   pinMode(MOT_SX_STEP, OUTPUT);
 }
 
-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 t = INT_MAX;
-int32_t period = INT_MAX, halfperiod1 = INT_MAX;
-
-void loop1(){
+void loop1() {
+  static bool sx_b = true, dx_b = true;
+  static uint32_t last_time_sx = micros(), last_time_dx = micros(), current_time_motors = micros();
   // 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
-
+  static int32_t tmotsx_period = INT_MAX, tmotdx_period = INT_MAX;
+  static uint32_t motsx_period = INT_MAX, motdx_period = INT_MAX;
   // Retrieve the half period value from core0. Non blocking call
-  if(rp2040.fifo.available()) {
-    rp2040.fifo.pop_nb(&t);
 
-    int32_t period = (int32_t)t;
+  if (rp2040.fifo.available() > 2) {
+    uint32_t t;
+    rp2040.fifo.pop_nb(&t);
+    tmotsx_period = (int32_t) t;
+    rp2040.fifo.pop_nb(&t);
+    tmotdx_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);
+  if (tmotsx_period > 0) {
     digitalWriteFast(MOT_SX_DIR, HIGH);
-      halfperiod1 = (uint32_t)(period);
-    }else{
+    motsx_period = (uint32_t)(tmotsx_period);
+  } else {
+    // Negative direction
+    digitalWriteFast(MOT_SX_DIR, LOW);
+    motsx_period = (uint32_t)(-tmotsx_period);
+  }
+  if (tmotdx_period > 0) {
+    digitalWriteFast(MOT_DX_DIR, HIGH);
+    motdx_period = (uint32_t)(tmotdx_period);
+  } else {
     // Negative direction
     digitalWriteFast(MOT_DX_DIR, LOW);
-      digitalWriteFast(MOT_SX_DIR, LOW);
-      halfperiod1 = (uint32_t)(-period);
-    }
+    motdx_period = (uint32_t)(-tmotdx_period);
   }
 
   current_time_motors = micros();
-  if(current_time_motors - last_time_motors > halfperiod1){
+  if (current_time_motors - last_time_sx > motsx_period) {
     // Half a pulse. Next cycle will be the rest
-    digitalWriteFast(MOT_DX_STEP, b);
-    digitalWriteFast(MOT_SX_STEP, b);
-    b = !b;
-    last_time_motors = current_time_motors;
+    digitalWriteFast(MOT_SX_STEP, sx_b);
+    sx_b = !sx_b;
+    last_time_sx = current_time_motors;
+  }
+  if (current_time_motors - last_time_dx > motdx_period) {
+    // Half a pulse. Next cycle will be the rest
+    digitalWriteFast(MOT_DX_STEP, dx_b);
+    dx_b = !dx_b;
+    last_time_dx = current_time_motors;
   }
 }
 
@@ -117,19 +157,20 @@ uint32_t current_time = millis(), last_time = millis();
 void loop() {
   update_imu();
 
-  input = pitch;
-  //if(abs(input -setpoint) <= 0.01) input = setpoint;
+  velCtrl.Compute();
+  yawCtrl.Compute();
 
-  // 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()){
+  if (pitchCtrl.Compute()) {
+    double tvelocity = angle_output;
 
-    double tvelocity = output;
-    tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
-    frequency = tvelocity *  0.1125;
-    half_period0 = 1000000 / (2*frequency);
+    frequency = (tvelocity + yaw_output) * CONVERSION_FACTOR * ANGLE_PER_STEP ;
+    half_period0 = 1000000 / (2 * frequency);
+    // Send the half period of one motor to core 1. Non blocking
+    rp2040.fifo.push_nb(half_period0);
 
-    // Send the half period to core 1. Non blocking
+    frequency = (tvelocity - yaw_output) * CONVERSION_FACTOR * ANGLE_PER_STEP ;
+    half_period0 = 1000000 / (2 * frequency);
+    // Send the half period of the other motor to core 1. Non blocking
     rp2040.fifo.push_nb(half_period0);
   }
-
 }