diff --git a/Main/IMU.ino b/Main/IMU.ino index d62f40d..b42cf88 100644 --- a/Main/IMU.ino +++ b/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,11 +47,10 @@ void readIMU() { //Complementary filter - dAngle = pitch_rate * dT_s; - pitch = acc_pitch * (1 - alpha) + ((dAngle + pitch_prev) * alpha); + pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); pitch_prev = pitch; - + } diff --git a/Main/Main.ino b/Main/Main.ino index cca747c..3f10e99 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -29,8 +29,8 @@ float dT_s = 0.0; //Motor variables -const int PWM_CYCLE = 12000; -const byte PWM_RESOLUTION = 12; +const int PWM_CYCLE = 12000; +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(2, PWM_CYCLE, PWM_RESOLUTION); - ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION); - ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION); - + ledcSetup(1, PWM_CYCLE, PWM_RES); + ledcSetup(2, PWM_CYCLE, PWM_RES); + ledcSetup(3, PWM_CYCLE, PWM_RES); + 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); } diff --git a/Main/interruptRemote.ino b/Main/interruptRemote.ino index e8afc38..0dbfb08 100644 --- a/Main/interruptRemote.ino +++ b/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; +const byte NO_CHANNELS = 2; +const byte CHANNEL_PINS[] = {12, 14}; +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 ch1_interrupt() { + interruptTime_ch1 = micros(); + if (interruptTime_ch1 >interruptTimeLast_ch1 && (interruptTime_ch1 - interruptTimeLast_ch1)< 2100){ + pwm_time_ch1 = interruptTime_ch1 - interruptTimeLast_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; + 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); @@ -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; } diff --git a/Main/motorControl.ino b/Main/motorControl.ino index 2e23ea3..417f26c 100644 --- a/Main/motorControl.ino +++ b/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_TC = 100.0; -const float K_OL = 13.0; -const float K_IL = 85.0; -const float I_IL = 5.25; -const float filter_gain = 16.0; +const float K_SC = 18.0; //Speed controller gain +const float K_TC = 130.0; //Turn controller gain +const float K_OL = 14.0; //Outer loop balance controller gain +const float K_IL = 85.0; //Inner loop balance controller gain +const float I_IL = 5.25; //Inner loop balance controller Igain +const float filter_gain = 16.0; //Motor speed LPF gain //Help variables @@ -37,10 +37,11 @@ mtx_type inv_Kin [2][2]; void initMotors() { - inv_Kin[0][0] = WHEEL_DIAMETER / 4; - inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH; - inv_Kin[0][1] = WHEEL_DIAMETER / 4; - inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH; + // 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; + inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH; } void motors() { @@ -56,42 +57,32 @@ void motors() { // Remote control commands - rem_turn_speed_ref = remoteCMD[0]; - rem_speed_ref = remoteCMD[1]; + rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2007.0, -3.5, 3.5); + rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 1997.0, -0.25, 0.25); + // Speed Controller - // 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; + SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); // Balance controller // Outer loop - ref_OL = BALANCE_POINT - SC_cont_out; - act_OL = pitch; - error_OL = ref_OL - act_OL; - OL_cont_out = error_OL * K_OL; + OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); // Inner loop - ref_IL = OL_cont_out; - act_IL = pitch_rate; - error_IL = ref_IL - act_IL; - iError_IL = iError_IL + (error_IL * dT_s * I_IL); - IL_cont_out = round((error_IL * K_IL) + iError_IL); + ref_IL = OL_cont_out; + act_IL = pitch_rate; + error_IL = ref_IL - act_IL; + iError_IL = iError_IL + (error_IL * dT_s * I_IL); + IL_cont_out = round((error_IL * K_IL) + iError_IL); //Turn controller - // 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; + TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC); //Sum speed command for motors - M1_Speed_CMD = IL_cont_out - TC_cont_out; - M2_Speed_CMD = IL_cont_out + TC_cont_out; + 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; @@ -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]