perform calibration

Achieved with a PI controller checking the current velocity as output by the motors. Since I'm
using stepper motors, the angular velocity is exact (if the motors are not skipping steps) and does not need encoders on the
motors.

TODO: add a little deadzone either on the setpoint or on the velocity, to prevent
oscillations when around the equilibrium point.

selfbalance-madgwick/imu.ino

@@ -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

@@ -22,18 +22,25 @@ 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;
+// PI constants for outer velocity control loop
+constexpr double KP_vel = 0.01;
+constexpr double KI_vel = 0.005;
+
+float angle_setpoint = 0.0;
+float angle_output = 0;
+float angle_input = 0;
+
+float vel_setpoint = 0.0;
+float vel_input = 0.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(&angle_input, &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::pOnMeas, velCtrl.dMode::dOnMeas, velCtrl.iAwMode::iAwCondition, velCtrl.Action::direct);
 
 void setup() {
   Serial.begin(9600);
@@ -48,7 +55,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,6 +64,10 @@ void setup() {
   pitchCtrl.SetMode(pitchCtrl.Control::automatic);
   pitchCtrl.SetSampleTimeUs(1000);
 
+  velCtrl.SetOutputLimits(-0.35, 0.35);
+  velCtrl.SetMode(velCtrl.Control::automatic);
+  velCtrl.SetSampleTimeUs(10000);
+
   digitalWrite(LED_BUILTIN, LOW);
 }
 
@@ -111,19 +122,18 @@ void loop1(){
 double frequency = 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()){
+  velCtrl.Compute();
 
-    double tvelocity = output;
+  angle_input = pitch;
+  if(pitchCtrl.Compute()){
+    double tvelocity = angle_output;
+    
     tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
     frequency = tvelocity *  0.1125;
     half_period0 = 1000000 / (2*frequency);