From 9c363bbe3c2c1bf8ed82a9ff1dc41480da18a163 Mon Sep 17 00:00:00 2001 From: Stedd Date: Mon, 23 Dec 2019 21:00:06 +0100 Subject: [PATCH] Added remote control --- Main/Main.ino | 21 ++++++----- Main/interruptRemote.ino | 69 +++++++++++++++++++++++++++++++++++ Main/motorControl.ino | 23 ++++++++++-- Main/plot.ino | 78 ++++++++++++++++++++++------------------ 4 files changed, 145 insertions(+), 46 deletions(-) create mode 100644 Main/interruptRemote.ino diff --git a/Main/Main.ino b/Main/Main.ino index da2ae1a..cca747c 100644 --- a/Main/Main.ino +++ b/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 diff --git a/Main/interruptRemote.ino b/Main/interruptRemote.ino new file mode 100644 index 0000000..e8afc38 --- /dev/null +++ b/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; +} diff --git a/Main/motorControl.ino b/Main/motorControl.ino index 94bef12..2e23ea3 100644 --- a/Main/motorControl.ino +++ b/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); diff --git a/Main/plot.ino b/Main/plot.ino index 3c76bd8..8f58c75 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -1,27 +1,42 @@ void plot(){ - // Time - // Serial.print("dT:"); - // Serial.print(dT); - // Serial.print(" "); - // Serial.print("dT_s:"); - // Serial.println(dT_s); + // Serial.print("dT:"); + // Serial.println(dT); + // Serial.print(" "); + // Serial.print("dT_s:"); + // Serial.println(dT_s); + // Serial.print(" "); // 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); + // 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); + // 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(" "); - // 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("M1_Ang_Vel:"); + // Serial.print(motor_ang_vel[0][0]); // Serial.print(" "); - // Serial.print("m2Raw:"); - // Serial.println(m2Raw); + // Serial.print("M2_Ang_Vel:"); + // 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]); }