Moved plots to separate function.

Cleanup

Main/IMU.ino

@@ -30,15 +30,9 @@ void readIMU() {
 
 
   //  Gyrocope
-  //  Coordinate system
+  gx  = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value);    //  gx -  Pitch rate
-  //  gx -  Pitch rate
+  gy  = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value);    //  gy -  Roll rate
-  //  gy -  Roll rate
+  gz  = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value);    //  gz -  Yaw rate
-  //  gz -  Yaw rate
-  //  Gyro is calibrated for +-2000deg/s
-  //  Conversion is happening in GY_85.h line 48-50
-  gx  = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value);
-  gy  = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value);
-  gz  = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value);
 
 
   //Temperature sensor
@@ -59,19 +53,6 @@ void readIMU() {
   pitch_prev  = pitch;
 
   
-  //Serial plotter
-  //  Serial.print  ( "Pitch:" );
-  //  Serial.print  ( pitch );
-  //  Serial.print  (" ");
-  //  Serial.print  ( "Accelerometer_Pitch:" );
-  //  Serial.print  ( acc_pitch );
-  //  Serial.print  (" ");
-  //  Serial.print  ( "," );
-  //  Serial.println  ( gz );
-  //  Serial.print  ( "," );
-  //  Serial.println  ( gt );
-  //  Serial.print  ( "," );
-  //  Serial.println  ( acc_pitch);
 }
 
 

Main/Main.ino

@@ -47,10 +47,6 @@ mtx_type      inv_Kin           [2][2];
 
 
 //Interrupt routines
-//  CW =  INCREASE
-//  CCW = DECREASE
-
-
 void IRAM_ATTR m1_A_changed() {
   M1_A_state = digitalRead(M1_ENC_A);
   M1_B_state = digitalRead(M1_ENC_B);
@@ -210,24 +206,21 @@ void loop() {
   dT    = tNow - tLast;             //[Cycle time in microseconds]
   dT_s  = dT * pow(10,-6);          //[Cycle time in seconds]
 
-//  Serial.print("dT:");
-//  Serial.print(dT);
-//  Serial.print(" ");
-//  Serial.print("dT_s:");
-//  Serial.println(dT_s);
-
 
   //Get sensor data
   readIMU();
 
 
-  //Control motor
+  //Control motors
   motors();
 
 
-  //Save time
+  //Save time for next cycle
   tLast = tNow;
 
+  //Plot
+  // plot();
+
 
   //Delay
   delay(5);             // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz

Main/motorControl.ino

@@ -5,6 +5,7 @@ const float   WHEEL_DIAMETER    = 0.0677;
 const float   PULSES_PER_TURN   = 1320.0;
 const float   BALANCE_POINT     = 0.05;
 const float   SPEED_REF         = 0.00;
+const float   TURN_SPEED_REF    = 0.00;
 const float   DEADBAND_M1_POS   = 90.0;
 const float   DEADBAND_M1_NEG   = 90.0;
 const float   DEADBAND_M2_POS   = 90.0;
@@ -85,39 +86,10 @@ void motors() {
   motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
 
 
-  //  Serial plotter
-  //  Serial.print("Balance_Point:");
-  //  Serial.print(ref_OL);
-  //  Serial.print(" ");
-  //  Serial.print("Pitch_Angle:");
-  //  Serial.print(act_OL);
-  //  Serial.print(" ");
-  //  Serial.print("Speed_CMD:");
-  //  Serial.println(Speed_CMD * (100.0 / 4096.0));
-
-
-  //  Serial.print("M1_Ang_Vel:");
-  //  Serial.print(M1_Ang_Vel);
-  //  Serial.print(" ");
-  //  Serial.print("M2_Ang_Vel:");
-  //  Serial.print(M2_Ang_Vel);
-  //  Serial.print(" ");
-  //  Serial.print("botLinVel:");
-  //  Serial.print(vel_Matrix[0][0]);
-  //  Serial.print(" ");
-  //  Serial.print("botAngVel:");
-  //  Serial.println(vel_Matrix[1][0]);
-
-
   //Update variables for next scan cycle
   m1RawLast = m1Raw;
   m2RawLast = m2Raw;
 
-  //  Serial.print("m1Raw:");
-  //  Serial.print(m1Raw);
-  //  Serial.print(" ");
-  //  Serial.print("m2Raw:");
-  //  Serial.println(m2Raw);
 
 }
 
@@ -137,9 +109,8 @@ float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, f
 
 void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) {
   //Calculate channel
-  byte ch1 = motorID * 2 - 1;
   byte ch2 = motorID * 2;
-
+  byte ch1 = ch2 - 1;
 
   //Deadband
   if (speedCMD_ > 0 && speedCMD_ < dbPos_) {

Main/plotter.ino

@@ -0,0 +1,54 @@
+void plot(){
+
+// Time
+  //  Serial.print("dT:");
+  //  Serial.print(dT);
+  //  Serial.print(" ");
+  //  Serial.print("dT_s:");
+  //  Serial.println(dT_s);
+
+// IMU
+  //  Serial.print  ( "Pitch:" );
+  //  Serial.print  ( pitch );
+  //  Serial.print  (" ");
+  //  Serial.print  ( "Accelerometer_Pitch:" );
+  //  Serial.print  ( acc_pitch );
+  //  Serial.print  (" ");
+  //  Serial.print  ( "," );
+  //  Serial.println  ( gz );
+  //  Serial.print  ( "," );
+  //  Serial.println  ( gt );
+  //  Serial.print  ( "," );
+  //  Serial.println  ( acc_pitch);
+
+
+// Motors
+  //  Serial plotter
+  //  Serial.print("Balance_Point:");
+  //  Serial.print(ref_OL);
+  //  Serial.print(" ");
+  //  Serial.print("Pitch_Angle:");
+  //  Serial.print(act_OL);
+  //  Serial.print(" ");
+  //  Serial.print("Speed_CMD:");
+  //  Serial.println(Speed_CMD * (100.0 / 4096.0));
+
+  //  Serial.print("M1_Ang_Vel:");
+  //  Serial.print(M1_Ang_Vel);
+  //  Serial.print(" ");
+  //  Serial.print("M2_Ang_Vel:");
+  //  Serial.print(M2_Ang_Vel);
+  //  Serial.print(" ");
+  //  Serial.print("botLinVel:");
+  //  Serial.print(vel_Matrix[0][0]);
+  //  Serial.print(" ");
+  //  Serial.print("botAngVel:");
+  //  Serial.println(vel_Matrix[1][0]);
+
+// Encoders
+   // Serial.print("m1Raw:");
+   // Serial.print(m1Raw);
+   // Serial.print(" ");
+   // Serial.print("m2Raw:");
+   // Serial.println(m2Raw);
+}