Formatting

motorControl.ino

@@ -13,7 +13,7 @@ const float DEADBAND_M2_NEG = 90.0;
 
 
 //Tuning
-const float gainScale = 0.75;
+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
@@ -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

@@ -103,12 +103,19 @@ 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) {
@@ -117,6 +124,14 @@ int PackInt(int _i, int value) {
   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];
@@ -131,6 +146,8 @@ int PackFloat(int _i, float value) {
   return _i + 4;
 }
 
+
+
 // int PackFloat(int _i, float value) {
 //   data[_i] = (value & 0x000000FF) ;
 //   data[_i + 2] = (value & 0x0000FF00)>>8;