Balance Controller kind of works

Not sure why the gains dont work anymore.
Anyways, continuing with UDP publishing.

Balancebot.ino

@@ -45,18 +45,19 @@ const char* _ps3Address = "18:5e:0f:92:00:6c";
 
 void setup() {
   //Initialize serial
-  Serial.begin(57600);
+  Serial.begin(9600);
   delay(10);
 
   //Initialice I2C
   Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
-  //delay(10);
+  delay(10);
+
 
   //Initialize IMU
   Serial.println("Before IMU init");
   IMU.init();
-  //IMU.init();
   Serial.println("After IMU init");
+
   delay(10);
 
   //Initialize encoder interrupts

IMU.ino

@@ -7,7 +7,7 @@ const int gyro_overflow_value = 4558;  // 4096+512-50=4558 ?
 //IMU VARIABLES
 int ax, ay, az;
 int cx, cy, cz;
-float gx, gy, gz;
+int gx, gy, gz;
 float gt;
 float acc_pitch;
 float pitch_rate;
@@ -16,30 +16,26 @@ float pitch_prev = 0;
 
 
 void readIMU() {
-  // Serial.println("ReadingIMU");
   //Acceletometer
-  int* accelerometerReadings = IMU.readFromAccelerometer();
-  ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value);
-  ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value);
-  az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value);
+  ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);
+  ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value);
+  az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value);
 
 
   //Magnetometer
-  int* compassReadings = IMU.readFromCompass();
-  cx = IMU.compass_x(compassReadings);
-  cy = IMU.compass_y(compassReadings);
-  cz = IMU.compass_z(compassReadings);
+  cx = IMU.compass_x(IMU.readFromCompass());
+  cy = IMU.compass_y(IMU.readFromCompass());
+  cz = IMU.compass_z(IMU.readFromCompass());
 
 
-  // //  Gyrocope
-  // float* gyroReadings = IMU.readGyro();
-  // gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value);  //  gx -  Pitch rate
-  // gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value);  //  gy -  Roll rate
-  // gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value);  //  gz -  Yaw rate
+  //  Gyrocope
+  gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value);  //  gx -  Pitch rate
+  gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value);  //  gy -  Roll rate
+  gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value);  //  gz -  Yaw rate
 
 
-  // //Temperature sensor
-  // gt = IMU.temp(gyroReadings);
+  //Temperature sensor
+  gt = IMU.temp(IMU.readGyro());
 
 
   // Pitch angle from accelerometer

motorControl.ino

@@ -13,14 +13,14 @@ const float DEADBAND_M2_NEG = 90.0;
 
 
 //Tuning
-const float K_SC = 18.5;         //Speed controller gain
-const float K_TC = 90.0;         //Turn controller gain
-const float K_OL = 13.0;         //Outer loop balance controller gain
-const float K_IL = 72.0;         //Inner loop balance controller gain
-const float I_IL = 80.0;         //Inner loop balance controller Igain
+const float gainScale = 0.75;
+const float K_SC = 18.5*gainScale;         //Speed controller gain
+const float K_TC = 90.0*gainScale;         //Turn controller gain
+const float K_OL = 13.0*gainScale;         //Outer loop balance controller gain
+const float K_IL = 72.0*gainScale;         //Inner loop balance controller gain
+const float I_IL = 80.0*gainScale;         //Inner loop balance controller Igain
 const float filter_gain = 16.0;  //Motor speed LPF gain
 
-
 //Help variables
 int M1_Speed_CMD, M2_Speed_CMD;
 float rem_speed_ref, rem_turn_speed_ref;
@@ -30,6 +30,7 @@ float OL_cont_out;
 float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
 float speedCmd1, speedCmd2;
 
+bool balancingOn = true;
 
 //Matrices
 mtx_type motor_ang_vel[2][1];
@@ -47,6 +48,24 @@ void initMotors() {
 
 void motors() {
 
+  if (Ps3.data.button.cross) {
+    ResetIntegrators();
+    balancingOn = true;
+  }
+
+  if (Ps3.data.button.circle) {
+    balancingOn = false;
+  }
+
+  if (Ps3.data.button.triangle) {
+    ResetIntegrators();
+  }
+
+  if (Ps3.data.button.square) {
+      IMU.init();
+  }
+
+  if (balancingOn) {
 
     //Calculate wheel angular velocity
     motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
@@ -76,13 +95,19 @@ void motors() {
 
 
     //Turn controller
-  TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[1][0], K_TC);
-
+    TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
 
     //Sum speed command for motors
     M1_Speed_CMD = IL_cont_out - TC_cont_out;
     M2_Speed_CMD = IL_cont_out + TC_cont_out;
 
+    //Motor control
+    IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
+    IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
+    IL_anti_windup = IL_anti_windup / 2;
+
+  } else {
+
     //Sum speed command for motors
     speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0);
     M1_Speed_CMD = MOTOR_SATURATION * speedCmd1;
@@ -91,17 +116,19 @@ void motors() {
     speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0);
     M2_Speed_CMD = MOTOR_SATURATION * speedCmd2;
     motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
+  }
 
-  //Motor control
-  // IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
-  // IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
-  // IL_anti_windup = IL_anti_windup / 2;
 
   //Update variables for next scan cycle
   m1RawLast = m1Raw;
   m2RawLast = m2Raw;
 }
 
+void ResetIntegrators() {
+  iError_IL = 0.0;
+  IL_anti_windup = 0.0;
+}
+
 float PController(float ref_, float act_, float k_) {
   return (ref_ - act_) * k_;
 }
@@ -151,6 +178,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
     speedCMD_ = speedCMD_;
   }
 
+
   //Apply speed command to PWM output
   if (speedCMD_ > 0) {
     ledcWrite(ch1, 0);

plot.ino

@@ -8,14 +8,15 @@ void plot() {
   // Serial.print(" ");
 
   // IMU
-  Serial.print  ( "Pitch:" );
-  Serial.println  ( pitch );
-  // Serial.print  (" ");
-  // Serial.print  ( "Accelerometer_Pitch:" );
-  // Serial.print  ( acc_pitch );
-  // Serial.print  (" ");
-  // Serial.print  ( "," );
-  // Serial.println  ( gz );
+
+  // Serial.print("RollRate:");
+  // Serial.println(pitch_rate);
+
+  // Serial.print("Accelerometer_Pitch:");
+  // Serial.println(acc_pitch);
+
+  // Serial.print("Pitch:");
+  // Serial.println(pitch);
   // Serial.print  ( "," );
   // Serial.println  ( gt );
   // Serial.print  ( " " );
@@ -37,30 +38,27 @@ void plot() {
 
   // Encoders
   // Serial.print("m1Raw:");
-  // Serial.print(m1Raw);
-  // Serial.print(" ");
+  // Serial.println(m1Raw);
+
   // Serial.print("m2Raw:");
   // Serial.println(m2Raw);
 
-  // Motors
+  // // Motors
   // Serial.print("SpeedControllerOut:");
-  // Serial.print(SC_cont_out);
-  // Serial.print(" ");
+  // Serial.println(SC_cont_out);
+
   // Serial.print("BalanceOLControllerOut:");
-  // Serial.print(OL_cont_out);
-  // Serial.print(" ");
+  // Serial.println(OL_cont_out);
+
   // Serial.print("BalanceILControllerOut:");
-  // Serial.print(IL_cont_out);
-  // Serial.print(" ");
-  // Serial.print("SpeedCmd1:");
-  // Serial.println(speedCmd1);
-  // Serial.print(" ");
+  // Serial.println(IL_cont_out);
+
+  // Serial.print("TurnControllerOut:");
+  // Serial.println(TC_cont_out);
+
   // Serial.print("M1_CMD:");
-  // Serial.print(M1_Speed_CMD);
-  // Serial.print(" ");
-  // Serial.print("SpeedCmd2:");
-  // Serial.println(speedCmd2);
-  // Serial.print(" ");
+  // Serial.println(M1_Speed_CMD);
+
   // Serial.print("M2_CMD:");
   // Serial.println(M2_Speed_CMD);
 
@@ -71,7 +69,7 @@ void plot() {
   // Serial.print(motor_ang_vel[0][1]);
   // Serial.print(" ");
   // Serial.print("botLinVel:");
-  // Serial.print(vel_Matrix[0][0]);
+  // Serial.println(vel_Matrix[0][0]);
   // Serial.print(" ");
   // Serial.print("botAngVel:");
   // Serial.println(vel_Matrix[1][0]);