Added remote control

Main/Main.ino

@@ -40,12 +40,6 @@ volatile bool   M1_A_state, M1_B_state;
 volatile bool   M2_A_state, M2_B_state;
 
 
-//Matrices
-mtx_type      motor_ang_vel   [2][1];
-mtx_type      vel_Matrix      [2][1];
-mtx_type      inv_Kin         [2][2];
-
-
 void setup() {
   //Initialize serial
   Serial.begin(57600);
@@ -71,6 +65,7 @@ void setup() {
   m2Raw = 0;
   m2RawLast = 100;
 
+
   // Initialize PWM channels
   ledcAttachPin(M1_A, 1);
   ledcAttachPin(M1_B, 2);
@@ -82,9 +77,14 @@ void setup() {
   ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
   ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
 
+
   //Initialize differential drive inverse kinematics
   initMotors();
 
+
+  // Initialize Remote control
+  initRemote();
+
 }
 
 void loop() {
@@ -98,6 +98,10 @@ void loop() {
   readIMU();
 
 
+  //Get remote control data
+  readRemote();
+
+
   //Control motors
   motors();
 
@@ -105,8 +109,9 @@ void loop() {
   //Save time for next cycle
   tLast = tNow;
 
-  //Plot
+
-  // plot();
+  // Plot
+  plot();
 
 
   //Delay

Main/interruptRemote.ino

@@ -0,0 +1,69 @@
+//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() {
+    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() {
+    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);
+
+  //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

@@ -14,7 +14,7 @@ const float   DEADBAND_M2_NEG   = 90.0;
 
 //Tuning
 const float   K_SC              = 20.0;
-const float   K_TC              = 50.0;
+const float   K_TC              = 100.0;
 const float   K_OL              = 13.0;
 const float   K_IL              = 85.0;
 const float   I_IL              = 5.25;
@@ -23,12 +23,19 @@ const float   filter_gain       = 16.0;
 
 //Help variables
 int           M1_Speed_CMD, M2_Speed_CMD;
+float         rem_speed_ref, rem_turn_speed_ref;
 float         ref_SC, act_SC, error_SC, SC_cont_out;
 float         ref_TC, act_TC, error_TC, TC_cont_out;
 float         ref_OL, act_OL, error_OL, OL_cont_out;
 float         ref_IL, act_IL, error_IL, IL_cont_out, iError_IL;
 
 
+//Matrices
+mtx_type      motor_ang_vel   [2][1];
+mtx_type      vel_Matrix      [2][1];
+mtx_type      inv_Kin         [2][2];
+
+
 void initMotors() {
   inv_Kin[0][0] = WHEEL_DIAMETER / 4;
   inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
@@ -48,8 +55,13 @@ void motors() {
   Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
 
 
+  // Remote control commands
+  rem_turn_speed_ref  = remoteCMD[0];
+  rem_speed_ref       = remoteCMD[1];
+
   // Speed Controller
-  ref_SC          = SPEED_REF;
+  // ref_SC          = SPEED_REF;
+  ref_SC          = rem_speed_ref;
   act_SC          = vel_Matrix[0][0];
   error_SC        = ref_SC - act_SC;
   SC_cont_out     = error_SC * K_SC;
@@ -70,7 +82,8 @@ void motors() {
 
 
   //Turn controller
-  ref_TC          = TURN_SPEED_REF;
+  // ref_TC          = TURN_SPEED_REF;
+  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;
@@ -80,6 +93,10 @@ void motors() {
   M1_Speed_CMD    = IL_cont_out - TC_cont_out;
   M2_Speed_CMD    = IL_cont_out + TC_cont_out;
 
+  //Sum speed command for motors
+  // M1_Speed_CMD    = 0;
+  // M2_Speed_CMD    = 0;
+
 
   //Motor control
   motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);

Main/plot.ino

@@ -1,27 +1,42 @@
 void plot(){
-
 // Time
-  //  Serial.print("dT:");
+   // Serial.print("dT:");
-  //  Serial.print(dT);
+   // Serial.println(dT);
-  //  Serial.print(" ");
+   // Serial.print(" ");
-  //  Serial.print("dT_s:");
+   // Serial.print("dT_s:");
-  //  Serial.println(dT_s);
+   // Serial.println(dT_s);
+      // Serial.print(" ");
 
 // IMU
-  //  Serial.print  ( "Pitch:" );
+   // Serial.print  ( "Pitch:" );
-  //  Serial.print  ( pitch );
+   // Serial.print  ( pitch );
-  //  Serial.print  (" ");
+   // Serial.print  (" ");
-  //  Serial.print  ( "Accelerometer_Pitch:" );
+   // Serial.print  ( "Accelerometer_Pitch:" );
-  //  Serial.print  ( acc_pitch );
+   // Serial.print  ( acc_pitch );
-  //  Serial.print  (" ");
+   // Serial.print  (" ");
-  //  Serial.print  ( "," );
+   // Serial.print  ( "," );
-  //  Serial.println  ( gz );
+   // Serial.println  ( gz );
-  //  Serial.print  ( "," );
+   // Serial.print  ( "," );
-  //  Serial.println  ( gt );
+   // Serial.println  ( gt );
-  //  Serial.print  ( "," );
+   // Serial.print  ( " " );
-  //  Serial.println  ( acc_pitch);
+   // Serial.println  ( acc_pitch);
 
 
+  // Remote control
+     // Serial.print("ch1:");
+     // Serial.print(remoteCMD[0]);
+     // Serial.print(" ");
+     // Serial.print("ch2:");
+     // Serial.println(remoteCMD[1]);
+
+
+  // Encoders
+     // Serial.print("m1Raw:");
+     // Serial.print(m1Raw);
+     // Serial.print(" ");
+     // Serial.print("m2Raw:");
+     // Serial.println(m2Raw);
+
 // Motors
   //  Serial plotter
   //  Serial.print("Balance_Point:");
@@ -33,22 +48,15 @@ void plot(){
   //  Serial.print("Speed_CMD:");
   //  Serial.println(Speed_CMD * (100.0 / 4096.0));
 
-  //  Serial.print("M1_Ang_Vel:");
+   // Serial.print("M1_Ang_Vel:");
-  //  Serial.print(M1_Ang_Vel);
+   // Serial.print(motor_ang_vel[0][0]);
-  //  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.print("M2_Ang_Vel:");
-   // Serial.println(m2Raw);
+   // Serial.print(motor_ang_vel[0][1]);
+   // Serial.print(" ");
+   // Serial.print("botLinVel:");
+   // Serial.print(vel_Matrix[0][0]);
+   // Serial.print(" ");
+   // Serial.print("botAngVel:");
+   // Serial.println(vel_Matrix[1][0]);
 }