Cleanup and tuning

Main/IMU.ino

@@ -11,7 +11,6 @@ int           gx, gy, gz;
 float         gt;
 float         acc_pitch;
 float         pitch_rate;
-float         dAngle, estAngle;
 float         pitch               = 0;
 float         pitch_prev          = 0;
 
@@ -48,8 +47,7 @@ void readIMU() {
 
 
   //Complementary filter
-  dAngle      = pitch_rate * dT_s;
+  pitch       = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
-  pitch       = acc_pitch * (1 - alpha) + ((dAngle + pitch_prev) * alpha);
   pitch_prev  = pitch;
 
 

Main/Main.ino

@@ -30,7 +30,7 @@ float         dT_s          = 0.0;
 
 //Motor variables
 const int     PWM_CYCLE     = 12000;
-const byte  PWM_RESOLUTION  = 12;
+const byte    PWM_RES       = 12;
 
 
 //Encoders variables
@@ -58,30 +58,31 @@ void setup() {
   //Initialize encoder interrupts
   initInterrupt();
 
-
   //Initialize encoders
   m1Raw = 0;
   m1RawLast = 100;
   m2Raw = 0;
   m2RawLast = 100;
 
-
   // Initialize PWM channels
+  // byte pwmPins[4]  = {M1_A, M1_B, M2_A, M2_B};
+  // for(int i = 1; i >= 4; i++){
+  //   ledcAttachPin(pwmPins[i-1], i);
+  //   ledcSetup(i, PWM_CYCLE, PWM_RES);
+  // }
   ledcAttachPin(M1_A, 1);
   ledcAttachPin(M1_B, 2);
   ledcAttachPin(M2_A, 3);
   ledcAttachPin(M2_B, 4);
 
-  ledcSetup(1, PWM_CYCLE, PWM_RESOLUTION);
+  ledcSetup(1, PWM_CYCLE, PWM_RES);
-  ledcSetup(2, PWM_CYCLE, PWM_RESOLUTION);
+  ledcSetup(2, PWM_CYCLE, PWM_RES);
-  ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
+  ledcSetup(3, PWM_CYCLE, PWM_RES);
-  ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
+  ledcSetup(4, PWM_CYCLE, PWM_RES);
-
 
   //Initialize differential drive inverse kinematics
   initMotors();
 
-
   // Initialize Remote control
   initRemote();
 
@@ -98,22 +99,18 @@ void loop() {
   readIMU();
 
 
-  //Get remote control data
-  readRemote();
-
-
   //Control motors
   motors();
 
 
-  //Save time for next cycle
-  tLast = tNow;
-
-
   // Plot
   plot();
 
 
+  //Save time for next cycle
+  tLast = tNow;
+
+
   //Delay
-  delay(5);             // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
+  delay(5);
 }

Main/interruptRemote.ino

@@ -1,37 +1,31 @@
 //Variables
 const     byte          NO_CHANNELS       = 2;
 const     byte          CHANNEL_PINS[]    = {12, 14};
-const int   CH_MIN            = 980;
-const int   CH_MAX            = 1997;
-const int   INIT_VALUE        = CH_MAX/2;
-float       remoteRaw[NO_CHANNELS];
-float       remoteCMD[NO_CHANNELS];
-
 volatile  unsigned int  interruptTime_ch1, interruptTimeLast_ch1;
 volatile  unsigned int  interruptTime_ch2, interruptTimeLast_ch2;
 volatile  unsigned int  pwm_time_ch1, pwm_time_ch2;
 
 
-  void ch1_interrupt() {
+void ch1_interrupt() {
   interruptTime_ch1 = micros();
   if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){
       pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_ch1;
   }
 
   interruptTimeLast_ch1 = interruptTime_ch1;
-  }
+}
 
-  void ch2_interrupt() {
+
+void ch2_interrupt() {
   interruptTime_ch2 = micros();
   if (interruptTime_ch2 > interruptTimeLast_ch2 && (interruptTime_ch2 - interruptTimeLast_ch2)< 2100 ){
       pwm_time_ch2 = interruptTime_ch2 - interruptTimeLast_ch2;
   }
   interruptTimeLast_ch2 = interruptTime_ch2;
-  }
+}
 
 
 void initRemote(){
-
   //Ch1
   pinMode(CHANNEL_PINS[0], INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[0]), ch1_interrupt, CHANGE);
@@ -39,31 +33,4 @@ void initRemote(){
   //Ch2
   pinMode(CHANNEL_PINS[1], INPUT);
   attachInterrupt(digitalPinToInterrupt(CHANNEL_PINS[1]), ch2_interrupt, CHANGE);
-
-}
-
-
-void readRemote(){
-
-  remoteCMD[0] = floatMap(pwm_time_ch1, 992.0, 2007.0, -2.5, 2.5); //turn rate
-  remoteCMD[1] = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25); //speed
-
-  // Remote control
-  // Serial.print("ch1:");
-  // Serial.print(pwm_time_ch1);
-  // Serial.print(" ");
-  // Serial.print("ch2:");
-  // Serial.print(pwm_time_ch2);
-  // Serial.print(" ");
-  // Serial.print("ch1_mapped:");
-  // Serial.print(remoteCMD[0]);
-  // Serial.print(" ");
-  // Serial.print("ch2_mapped:");
-  // Serial.println(remoteCMD[1]);
-
-}
-
-
-float floatMap(int in, float inMin, float inMax, float outMin, float outMax){
-  return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
 }

Main/motorControl.ino

@@ -1,5 +1,5 @@
 //Constants
-const int     MOTOR_SATURATION  = round(pow(2, PWM_RESOLUTION));
+const int     MOTOR_SATURATION  = round(pow(2, PWM_RES));
 const float   BASE_WIDTH        = 0.1837;
 const float   WHEEL_DIAMETER    = 0.0677;
 const float   PULSES_PER_TURN   = 1320.0;
@@ -13,12 +13,12 @@ const float   DEADBAND_M2_NEG   = 90.0;
 
 
 //Tuning
-const float   K_SC              = 20.0;
+const float   K_SC              = 18.0;   //Speed controller gain
-const float   K_TC              = 100.0;
+const float   K_TC              = 130.0;  //Turn controller gain
-const float   K_OL              = 13.0;
+const float   K_OL              = 14.0;   //Outer loop balance controller gain
-const float   K_IL              = 85.0;
+const float   K_IL              = 85.0;   //Inner loop balance controller gain
-const float   I_IL              = 5.25;
+const float   I_IL              = 5.25;   //Inner loop balance controller Igain
-const float   filter_gain       = 16.0;
+const float   filter_gain       = 16.0;   //Motor speed LPF gain
 
 
 //Help variables
@@ -37,6 +37,7 @@ mtx_type      inv_Kin         [2][2];
 
 
 void initMotors() {
+  // Inverse Kinematic matrix of differential drive robot
   inv_Kin[0][0]       = WHEEL_DIAMETER / 4;
   inv_Kin[1][0]       = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
   inv_Kin[0][1]       = WHEEL_DIAMETER / 4;
@@ -56,23 +57,17 @@ void motors() {
 
 
   // Remote control commands
-  rem_turn_speed_ref  = remoteCMD[0];
+  rem_turn_speed_ref  = floatMap(pwm_time_ch1, 992.0, 2007.0, -3.5, 3.5);
-  rem_speed_ref       = remoteCMD[1];
+  rem_speed_ref       = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25);
+
 
   // Speed Controller
-  // ref_SC          = SPEED_REF;
+  SC_cont_out         = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
-  ref_SC          = rem_speed_ref;
-  act_SC          = vel_Matrix[0][0];
-  error_SC        = ref_SC - act_SC;
-  SC_cont_out     = error_SC * K_SC;
 
 
   // Balance controller
   // Outer loop
-  ref_OL          = BALANCE_POINT - SC_cont_out;
+  OL_cont_out         = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
-  act_OL          = pitch;
-  error_OL        = ref_OL - act_OL;
-  OL_cont_out     = error_OL * K_OL;
   // Inner loop
   ref_IL              = OL_cont_out;
   act_IL              = pitch_rate;
@@ -82,11 +77,7 @@ void motors() {
 
 
   //Turn controller
-  // ref_TC          = TURN_SPEED_REF;
+  TC_cont_out         = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
-  ref_TC          = rem_turn_speed_ref;
-  act_TC          = vel_Matrix[0][1];
-  error_TC        = ref_TC - act_TC;
-  TC_cont_out     = error_TC * K_TC;
 
 
   //Sum speed command for motors
@@ -110,6 +101,15 @@ void motors() {
 
 }
 
+float PController(float ref_, float act_, float k_){
+  return (ref_-act_)*k_;
+}
+
+
+float floatMap(int in, float inMin, float inMax, float outMin, float outMax){
+  return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
+}
+
 float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) {
   float dEnc_     = encRaw - encRawLast;                                  //[Number of encoder pulses this cycle]
   float dTurn_    = dEnc_ / pulses_per_turn_;                             //[Amount wheel turned this cycle. 1 = full rotation]