Formatting

motorControl.ino

@@ -13,12 +13,12 @@ const float DEADBAND_M2_NEG = 90.0;
 
 
 //Tuning
-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 gainScale = 1;
+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
@@ -65,13 +65,10 @@ void motors() {
     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);
+    motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
     motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
-
-
   //Calculate robot linear and angular velocity
   Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
 
@@ -79,10 +76,12 @@ void motors() {
   rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75);
   rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35);
 
+  if (balancingOn) {
+
+
     // Speed Controller
     SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
 
-
     // Balance controller
     // Outer loop
     OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);

plot.ino

@@ -91,8 +91,8 @@ void plot() {
 
   int i = 0;
   data[i] = watchdog++;
-  data[i += 1] = balancingOn<<1;
-  i = PackInt(i+=1, M1_Speed_CMD);
+  data[i += 1] = balancingOn << 1;
+  i = PackInt(i += 1, M1_Speed_CMD);
   i = PackInt(i, M2_Speed_CMD);
   i = PackFloat(i, acc_pitch);
   i = PackFloat(i, pitch);
@@ -103,20 +103,35 @@ void plot() {
   i = PackFloat(i, TC_cont_out);
   i = PackFloat(i, OL_cont_out);
   i = PackFloat(i, ref_IL);
+  i = PackFloat(i, act_IL);
   i = PackFloat(i, error_IL);
   i = PackFloat(i, IL_cont_out);
   i = PackFloat(i, iError_IL);
   i = PackFloat(i, IL_anti_windup);
   i = PackFloat(i, speedCmd1);
   i = PackFloat(i, speedCmd2);
+  i = PackFloat(i, vel_Matrix[0][0]);
+  i = PackFloat(i, vel_Matrix[1][0]);
+  i = PackFloat(i, motor_ang_vel[0][0]);
+  i = PackFloat(i, motor_ang_vel[1][0]);
+  i = PackLong(i, m1Raw);
+  i = PackLong(i, m2Raw);
 }
 
 int PackInt(int _i, int value) {
-  data[_i] = (value & 0x00FF) ;
-  data[_i + 1] = (value & 0xFF00)>>8;  
+  data[_i] = (value & 0x00FF);
+  data[_i + 1] = (value & 0xFF00) >> 8;
   return _i + 2;
 }
 
+int PackLong(int _i, long value) {
+  data[_i] = (value & 0x000000FF);
+  data[_i + 1] = (value & 0x0000FF00) >> 8;
+  data[_i + 2] = (value & 0x00FF0000) >> 16;
+  data[_i + 3] = (value & 0xFF000000) >> 24;
+  return _i + 4;
+}
+
 union FloatToBytes {
   float value;
   byte bytes[4];
@@ -125,12 +140,14 @@ union FloatToBytes {
 int PackFloat(int _i, float value) {
   FloatToBytes converter;
   converter.value = value;
-    for(int j = 0; j < 4; j++) {
+  for (int j = 0; j < 4; j++) {
     data[_i + j] = converter.bytes[j];
   }
   return _i + 4;
 }
 
+
+
 // int PackFloat(int _i, float value) {
 //   data[_i] = (value & 0x000000FF) ;
 //   data[_i + 2] = (value & 0x0000FF00)>>8;