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,10 +36,26 @@ void setup_imu(){
     }
   }
 
+
+
 #ifdef PERFORM_CALIBRATION
-  delay(1500);
+  Serial.println("Keep IMU level.");
+  delay(500);
   IMU.calibrateAccelGyro(&calib);
-  delay(1500);
+  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);
   IMU.init(calib, IMU_ADDRESS);
 #endif
   filter.begin(0.2f);

selfbalance-madgwick/selfbalance-madgwick.ino

@@ -11,49 +11,29 @@ constexpr double ANGLE_PER_STEP = 0.1125;
 constexpr double WEIGHT = 0.961;
 constexpr double WHEEL_RADIUS = 0.0475;
 // Experimentally, the lowest pulse my steppers can handle without stalling + some leeway
-constexpr double MAX_HALF_PERIOD = 75;  // in microseconds
+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 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;
-
+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
+float setpoint = 0.0;
+float output = 0;
+float input = 0;
 
 float yaw{ 0 }, pitch{ 0 }, roll{ 0 };
 
 
 
-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);
-
+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);
@@ -61,14 +41,14 @@ void setup() {
   delay(1000);
 
   setup_imu();
-
+  
   // 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();
   }
 
@@ -77,74 +57,54 @@ 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);
+  pinMode(MOT_SX_DIR, OUTPUT); 
 
   pinMode(MOT_DX_STEP, OUTPUT);
   pinMode(MOT_SX_STEP, OUTPUT);
 }
 
-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();
+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(){
   // 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() > 2) {
-    uint32_t t;
+  if(rp2040.fifo.available()) {
     rp2040.fifo.pop_nb(&t);
-    tmotsx_period = (int32_t) t;
-    rp2040.fifo.pop_nb(&t);
-    tmotdx_period = (int32_t) 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 (tmotsx_period > 0) {
-    digitalWriteFast(MOT_SX_DIR, HIGH);
-    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);
-    motdx_period = (uint32_t)(-tmotdx_period);
+    // 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_sx > motsx_period) {
+  if(current_time_motors - last_time_motors > halfperiod1){
     // Half a pulse. Next cycle will be the rest
-    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;
+    digitalWriteFast(MOT_DX_STEP, b);
+    digitalWriteFast(MOT_SX_STEP, b);
+    b = !b;
+    last_time_motors = current_time_motors;
   }
 }
 
@@ -157,20 +117,19 @@ uint32_t current_time = millis(), last_time = millis();
 void loop() {
   update_imu();
 
-  velCtrl.Compute();
-  yawCtrl.Compute();
+  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()){
 
-  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);
-
-    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
+    // Send the half period to core 1. Non blocking
     rp2040.fifo.push_nb(half_period0);
   }
+
 }