Balance Controller kind of works

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

Balancebot.ino

@@ -45,7 +45,7 @@ const char* _ps3Address = "18:5e:0f:92:00:6c";
 
 void setup() {
   //Initialize serial
-  Serial.begin(19200);
+  Serial.begin(9600);
   delay(10);
 
   //Initialice I2C

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,7 +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 = false;
+bool balancingOn = true;
 
 //Matrices
 mtx_type motor_ang_vel[2][1];
@@ -61,6 +61,10 @@ void motors() {
     ResetIntegrators();
   }
 
+  if (Ps3.data.button.square) {
+      IMU.init();
+  }
+
   if (balancingOn) {
 
     //Calculate wheel angular velocity
@@ -92,6 +96,7 @@ void motors() {
 
     //Turn controller
     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;
@@ -173,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,20 +8,19 @@ void plot() {
   // Serial.print(" ");
 
   // IMU
-  Serial.print("Pitch:");
-  Serial.println(pitch);
+
+  // Serial.print("RollRate:");
+  // Serial.println(pitch_rate);
 
   // Serial.print("Accelerometer_Pitch:");
   // Serial.println(acc_pitch);
 
-  Serial.print("RollRate");
-  Serial.println(gz);
-
-  // Serial.print(",");
-  // Serial.println(gt);
-
-  // Serial.print(" ");
-  // Serial.println(acc_pitch);
+  // Serial.print("Pitch:");
+  // Serial.println(pitch);
+  // Serial.print  ( "," );
+  // Serial.println  ( gt );
+  // Serial.print  ( " " );
+  // Serial.println  ( acc_pitch);
 
 
   // Remote control
@@ -39,41 +38,35 @@ void plot() {
 
   // Encoders
   // Serial.print("m1Raw:");
-  // Serial.print(m1Raw);
-  // Serial.print(" ");
+  // Serial.println(m1Raw);
+
   // Serial.print("m2Raw:");
   // Serial.println(m2Raw);
 
-  // Motors
-  Serial.print("SpeedControllerOut:");
-  Serial.println(SC_cont_out);
+  // // Motors
+  // Serial.print("SpeedControllerOut:");
+  // Serial.println(SC_cont_out);
 
-  Serial.print("BalanceOLControllerOut:");
-  Serial.println(OL_cont_out);
+  // Serial.print("BalanceOLControllerOut:");
+  // Serial.println(OL_cont_out);
 
-  Serial.print("BalanceILControllerOut:");
-  Serial.println(IL_cont_out);
+  // Serial.print("BalanceILControllerOut:");
+  // Serial.println(IL_cont_out);
 
-  Serial.print("AntiWindup:");
-  Serial.println(IL_anti_windup);
+  // Serial.print("TurnControllerOut:");
+  // Serial.println(TC_cont_out);
 
-  // Serial.print("SpeedCmd1:");
-  // Serial.println(speedCmd1);
-  // Serial.print(" ");
   // Serial.print("M1_CMD:");
   // Serial.println(M1_Speed_CMD);
-  // Serial.print(" ");
-  // Serial.print("SpeedCmd2:");
-  // Serial.println(speedCmd2);
-  // Serial.print(" ");
+
   // Serial.print("M2_CMD:");
   // Serial.println(M2_Speed_CMD);
 
   // Serial.print("M1_Ang_Vel:");
-  // Serial.println(motor_ang_vel[0][0]);
+  // Serial.print(motor_ang_vel[0][0]);
   // Serial.print(" ");
   // Serial.print("M2_Ang_Vel:");
-  // Serial.println(motor_ang_vel[0][1]);
+  // Serial.print(motor_ang_vel[0][1]);
   // Serial.print(" ");
   // Serial.print("botLinVel:");
   // Serial.println(vel_Matrix[0][0]);