From 23c84d2e41e958fe8fb588bdbadc0cc62a193538 Mon Sep 17 00:00:00 2001 From: Stedd Date: Wed, 18 Oct 2023 21:48:18 +0200 Subject: [PATCH 01/22] Added PS3 controller --- Main/Main.ino | 9 ++++++--- Main/interruptRemote.ino | 36 ------------------------------------ Main/motorControl.ino | 14 +++----------- Main/plot.ino | 15 +++++++++++++++ 4 files changed, 24 insertions(+), 50 deletions(-) delete mode 100644 Main/interruptRemote.ino diff --git a/Main/Main.ino b/Main/Main.ino index b4dd646..f7c10e5 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -2,7 +2,8 @@ #include #include #include - +#include +#include //Declare library objects GY_85 IMU; @@ -39,6 +40,8 @@ long int m2Raw, m2RawLast; volatile bool M1_A_state, M1_B_state; volatile bool M2_A_state, M2_B_state; +//PS3 Controller variables +const char* _ps3Address = "18:5e:0f:92:00:6c"; void setup() { //Initialize serial @@ -78,8 +81,8 @@ void setup() { //Initialize differential drive inverse kinematics initMotors(); - // Initialize Remote control - initRemote(); + //Initialize PS3 controller connection + Ps3.begin(_ps3Address); } diff --git a/Main/interruptRemote.ino b/Main/interruptRemote.ino deleted file mode 100644 index 0dbfb08..0000000 --- a/Main/interruptRemote.ino +++ /dev/null @@ -1,36 +0,0 @@ -//Variables -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 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); -} diff --git a/Main/motorControl.ino b/Main/motorControl.ino index e753074..c958aa8 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -56,17 +56,9 @@ void motors() { //Calculate robot linear and angular velocity Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); - - // Remote control commands - if (pwm_time_ch1 == 0 && pwm_time_ch2 == 0){ - rem_turn_speed_ref = 0; - rem_speed_ref = 0; - } - else{ - rem_turn_speed_ref = floatMap(pwm_time_ch1, 992.0, 2015.0, -3.75, 3.75); - rem_speed_ref = floatMap(pwm_time_ch2, 982.0, 2005.0, -0.35, 0.35); - } - + //Get Control Commands + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); // Speed Controller SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); diff --git a/Main/plot.ino b/Main/plot.ino index c0ea6b0..96b5ce0 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -72,4 +72,19 @@ void plot(){ // Serial.print(" "); // Serial.print("botAngVel:"); // Serial.println(vel_Matrix[1][0]); + + // //PS3 Controller + // if (Ps3.isConnected()) { + // Serial.print("lx:"); + // Serial.print(Ps3.data.analog.stick.lx); + // Serial.print(","); + // Serial.print("ly:"); + // Serial.print(Ps3.data.analog.stick.ly); + // Serial.print(","); + // Serial.print("rx:"); + // Serial.print(Ps3.data.analog.stick.rx); + // Serial.print(","); + // Serial.print("ry:"); + // Serial.println(Ps3.data.analog.stick.ry); + // } } From b5a53fa8c9a863bc5f7eef0eed28b50530b2ad37 Mon Sep 17 00:00:00 2001 From: Stedd Date: Wed, 18 Oct 2023 22:16:53 +0200 Subject: [PATCH 02/22] Cleanup --- Main/IMU.ino | 50 +++++++------ Main/Main.ino | 57 ++++++++------- Main/interruptEncoders.ino | 35 ++++----- Main/motorControl.ino | 141 ++++++++++++++++++------------------- Main/plot.ino | 130 +++++++++++++++++----------------- 5 files changed, 199 insertions(+), 214 deletions(-) diff --git a/Main/IMU.ino b/Main/IMU.ino index b42cf88..83ff38b 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -1,41 +1,41 @@ //CONSTANTS -const float alpha = 0.95; -const int acc_overflow_value = 65535; -const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? +const float alpha = 0.95; +const int acc_overflow_value = 65535; +const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? //IMU VARIABLES -int ax, ay, az; -int cx, cy, cz; -int gx, gy, gz; -float gt; -float acc_pitch; -float pitch_rate; -float pitch = 0; -float pitch_prev = 0; +int ax, ay, az; +int cx, cy, cz; +int gx, gy, gz; +float gt; +float acc_pitch; +float pitch_rate; +float pitch = 0; +float pitch_prev = 0; void readIMU() { //Acceletometer - ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value); - ay = convertInt(IMU.accelerometer_y( IMU.readFromAccelerometer() ), acc_overflow_value); - az = convertInt(IMU.accelerometer_z( IMU.readFromAccelerometer() ), acc_overflow_value); + ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value); + ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value); + az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value); //Magnetometer - cx = IMU.compass_x( IMU.readFromCompass() ); - cy = IMU.compass_y( IMU.readFromCompass() ); - cz = IMU.compass_z( IMU.readFromCompass() ); + cx = IMU.compass_x(IMU.readFromCompass()); + cy = IMU.compass_y(IMU.readFromCompass()); + cz = IMU.compass_z(IMU.readFromCompass()); // Gyrocope - gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate - gy = convertInt(IMU.gyro_y( IMU.readGyro() ), gyro_overflow_value); // gy - Roll rate - gz = convertInt(IMU.gyro_z( IMU.readGyro() ), gyro_overflow_value); // gz - Yaw rate + gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate //Temperature sensor - gt = IMU.temp ( IMU.readGyro() ); + gt = IMU.temp(IMU.readGyro()); // Pitch angle from accelerometer @@ -47,10 +47,8 @@ void readIMU() { //Complementary filter - pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); - pitch_prev = pitch; - - + pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); + pitch_prev = pitch; } @@ -61,7 +59,7 @@ int convertInt(int raw, int overflow_value_) { return (raw - overflow_value_); } - else { + else { return raw; } } diff --git a/Main/Main.ino b/Main/Main.ino index f7c10e5..7a2c05c 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -6,39 +6,39 @@ #include //Declare library objects -GY_85 IMU; +GY_85 IMU; //GPIO PIN MAPPING -const byte M1_ENC_A = 32; -const byte M1_ENC_B = 33; -const byte M2_ENC_A = 34; -const byte M2_ENC_B = 35; -const byte M1_A = 16; -const byte M1_B = 17; -const byte M2_A = 18; -const byte M2_B = 19; -const byte IMU_I2C_SCL = 26; -const byte IMU_I2C_SDA = 27; +const byte M1_ENC_A = 32; +const byte M1_ENC_B = 33; +const byte M2_ENC_A = 34; +const byte M2_ENC_B = 35; +const byte M1_A = 16; +const byte M1_B = 17; +const byte M2_A = 18; +const byte M2_B = 19; +const byte IMU_I2C_SCL = 26; +const byte IMU_I2C_SDA = 27; //Time variables -unsigned long tNow = micros(); -unsigned long tLast = micros() + 13000; -int dT = 0; -float dT_s = 0.0; +unsigned long tNow = micros(); +unsigned long tLast = micros() + 13000; +int dT = 0; +float dT_s = 0.0; //Motor variables -const int PWM_CYCLE = 12000; -const byte PWM_RES = 12; +const int PWM_CYCLE = 12000; +const byte PWM_RES = 12; //Encoders variables -long int m1Raw, m1RawLast; -long int m2Raw, m2RawLast; -volatile bool M1_A_state, M1_B_state; -volatile bool M2_A_state, M2_B_state; +long int m1Raw, m1RawLast; +long int m2Raw, m2RawLast; +volatile bool M1_A_state, M1_B_state; +volatile bool M2_A_state, M2_B_state; //PS3 Controller variables const char* _ps3Address = "18:5e:0f:92:00:6c"; @@ -54,12 +54,10 @@ void setup() { //Initialize IMU IMU.init(); - //Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values... - IMU.GyroCalibrate(); delay(10); //Initialize encoder interrupts - initInterrupt(); + initEncoderInterrupt(); //Initialize encoders m1Raw = 0; @@ -83,14 +81,13 @@ void setup() { //Initialize PS3 controller connection Ps3.begin(_ps3Address); - } void loop() { //Update time variables - tNow = micros(); - dT = tNow - tLast; //[Cycle time in microseconds] - dT_s = dT * pow(10,-6); //[Cycle time in seconds] + tNow = micros(); + dT = tNow - tLast; //[Cycle time in microseconds] + dT_s = dT * pow(10, -6); //[Cycle time in seconds] //Get sensor data @@ -102,7 +99,7 @@ void loop() { // Plot - plot(); + //plot(); //Save time for next cycle @@ -111,4 +108,6 @@ void loop() { //Delay delay(5); + + //Test } diff --git a/Main/interruptEncoders.ino b/Main/interruptEncoders.ino index 29ca3a7..12aa1a7 100644 --- a/Main/interruptEncoders.ino +++ b/Main/interruptEncoders.ino @@ -7,8 +7,7 @@ void IRAM_ATTR m1_A_changed() { if (M1_A_state == HIGH) { if (M1_B_state == HIGH) { m1Raw = m1Raw - 1; - } - else if (M1_B_state == LOW) { + } else if (M1_B_state == LOW) { m1Raw = m1Raw + 1; } } @@ -17,8 +16,7 @@ void IRAM_ATTR m1_A_changed() { else if (M1_A_state == LOW) { if (M1_B_state == HIGH) { m1Raw = m1Raw + 1; - } - else if (M1_B_state == LOW) { + } else if (M1_B_state == LOW) { m1Raw = m1Raw - 1; } } @@ -33,8 +31,7 @@ void IRAM_ATTR m1_B_changed() { if (M1_B_state == HIGH) { if (M1_A_state == HIGH) { m1Raw = m1Raw + 1; - } - else if (M1_A_state == LOW) { + } else if (M1_A_state == LOW) { m1Raw = m1Raw - 1; } } @@ -43,8 +40,7 @@ void IRAM_ATTR m1_B_changed() { else if (M1_B_state == LOW) { if (M1_A_state == HIGH) { m1Raw = m1Raw - 1; - } - else if (M1_A_state == LOW) { + } else if (M1_A_state == LOW) { m1Raw = m1Raw + 1; } } @@ -58,8 +54,7 @@ void IRAM_ATTR m2_A_changed() { if (M2_A_state == HIGH) { if (M2_B_state == HIGH) { m2Raw = m2Raw + 1; - } - else if (M2_B_state == LOW) { + } else if (M2_B_state == LOW) { m2Raw = m2Raw - 1; } } @@ -68,8 +63,7 @@ void IRAM_ATTR m2_A_changed() { else if (M2_A_state == LOW) { if (M2_B_state == HIGH) { m2Raw = m2Raw - 1; - } - else if (M2_B_state == LOW) { + } else if (M2_B_state == LOW) { m2Raw = m2Raw + 1; } } @@ -84,8 +78,7 @@ void IRAM_ATTR m2_B_changed() { if (M2_B_state == HIGH) { if (M2_A_state == HIGH) { m2Raw = m2Raw - 1; - } - else if (M2_A_state == LOW) { + } else if (M2_A_state == LOW) { m2Raw = m2Raw + 1; } } @@ -94,21 +87,23 @@ void IRAM_ATTR m2_B_changed() { else if (M2_B_state == LOW) { if (M2_A_state == HIGH) { m2Raw = m2Raw + 1; - } - else if (M2_A_state == LOW) { + } else if (M2_A_state == LOW) { m2Raw = m2Raw - 1; } } } -void initInterrupt(){ +void initEncoderInterrupt() { pinMode(M1_ENC_A, INPUT_PULLUP); - pinMode(M1_ENC_B, INPUT_PULLUP); - pinMode(M2_ENC_A, INPUT_PULLUP); - pinMode(M2_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); + + pinMode(M1_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE); + + pinMode(M2_ENC_A, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE); + + pinMode(M2_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); } diff --git a/Main/motorControl.ino b/Main/motorControl.ino index c958aa8..60694ab 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -1,48 +1,48 @@ //Constants -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; -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; -const float DEADBAND_M2_NEG = 90.0; +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; +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; +const float DEADBAND_M2_NEG = 90.0; //Tuning -const float K_SC = 18.5; //Speed controller gain -const float K_TC = 90.0; //Turn controller gain -const float K_OL = 13.0; //Outer loop balance controller gain -const float K_IL = 72.0; //Inner loop balance controller gain -const float I_IL = 80.0; //Inner loop balance controller Igain -const float filter_gain = 16.0; //Motor speed LPF gain +const float K_SC = 18.5; //Speed controller gain +const float K_TC = 90.0; //Turn controller gain +const float K_OL = 13.0; //Outer loop balance controller gain +const float K_IL = 72.0; //Inner loop balance controller gain +const float I_IL = 80.0; //Inner loop balance controller Igain +const float filter_gain = 16.0; //Motor speed LPF gain //Help variables -int M1_Speed_CMD, M2_Speed_CMD; -float rem_speed_ref, rem_turn_speed_ref; -float SC_cont_out; -float TC_cont_out; -float OL_cont_out; -float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; +int M1_Speed_CMD, M2_Speed_CMD; +float rem_speed_ref, rem_turn_speed_ref; +float SC_cont_out; +float TC_cont_out; +float OL_cont_out; +float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; //Matrices -mtx_type motor_ang_vel [2][1]; -mtx_type vel_Matrix [2][1]; -mtx_type inv_Kin [2][2]; +mtx_type motor_ang_vel[2][1]; +mtx_type vel_Matrix[2][1]; +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; - inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH; + 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() { @@ -57,93 +57,89 @@ void motors() { Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); //Get Control Commands - rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); - rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); // Speed Controller - SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); + SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); // Balance controller // Outer loop - OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, 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 + (dT_s*(error_IL * I_IL) + (IL_anti_windup*((1/I_IL)+(1/K_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 + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL)))); + IL_cont_out = round((error_IL * K_IL) + iError_IL); //Turn controller - TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], 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; - // M2_Speed_CMD = 0; + M1_Speed_CMD = 0; + M2_Speed_CMD = 0; //Motor control IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); - IL_anti_windup = IL_anti_windup/2; + IL_anti_windup = IL_anti_windup / 2; //Update variables for next scan cycle m1RawLast = m1Raw; m2RawLast = m2Raw; - - } -float PController(float ref_, float act_, float k_){ - return (ref_-act_)*k_; +float PController(float ref_, float act_, float k_) { + return (ref_ - act_) * k_; } -float floatMap(int in, float inMin, float inMax, float outMin, float outMax){ +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] - float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_); - return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_); +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] + float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_); + return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_); } -float encoderReaderAngVel(int encRaw, int encRawLast, float ang_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] - float ang_vel_ = (dTurn_ * 2 * PI) / (dT_); - return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_); +float encoderReaderAngVel(int encRaw, int encRawLast, float ang_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] + float ang_vel_ = (dTurn_ * 2 * PI) / (dT_); + return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_); } float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { //Returns anti windup difference //Calculate channel - byte ch2 = motorID * 2; - byte ch1 = ch2 - 1; - float windup = 0; + byte ch2 = motorID * 2; + byte ch1 = ch2 - 1; + float windup = 0; //Deadband if (speedCMD_ > 0 && speedCMD_ < dbPos_) { speedCMD_ = dbPos_; - } - else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { + } else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { speedCMD_ = -dbNeg_; } // Speed command saturation else if (speedCMD_ > saturation) { - windup = saturation-speedCMD_; + windup = saturation - speedCMD_; speedCMD_ = saturation; - } - else if (speedCMD_ < -saturation) { - windup = saturation-speedCMD_; + } else if (speedCMD_ < -saturation) { + windup = saturation - speedCMD_; speedCMD_ = -saturation; } @@ -155,16 +151,13 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl if (speedCMD_ > 0) { ledcWrite(ch1, 0); ledcWrite(ch2, speedCMD_); - } - else if (speedCMD_ < 0) { + } else if (speedCMD_ < 0) { ledcWrite(ch1, -1 * speedCMD_); ledcWrite(ch2, 0); - } - else if (speedCMD_ == 0) { + } else if (speedCMD_ == 0) { ledcWrite(ch1, 0); ledcWrite(ch2, 0); } return windup; - } diff --git a/Main/plot.ino b/Main/plot.ino index 96b5ce0..5217571 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -1,77 +1,77 @@ -void plot(){ -// Time - // Serial.print("dT:"); - // Serial.println(dT); - // Serial.print(" "); - // Serial.print("dT_s:"); - // Serial.println(dT_s); - // Serial.print(" "); +void plot() { + // Time + // 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); + // 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); // Remote control - // Serial.print("ch1:"); - // Serial.print(pwm_time_ch1); - // Serial.print(" "); - // Serial.print("ch2:"); - // Serial.print(pwm_time_ch2); - // Serial.print("ch1mapped:"); - // Serial.print(rem_turn_speed_ref); - // Serial.print(" "); - // Serial.print("ch2mapped:"); - // Serial.println(rem_speed_ref); + // Serial.print("ch1:"); + // Serial.print(pwm_time_ch1); + // Serial.print(" "); + // Serial.print("ch2:"); + // Serial.print(pwm_time_ch2); + // Serial.print("ch1mapped:"); + // Serial.print(rem_turn_speed_ref); + // Serial.print(" "); + // Serial.print("ch2mapped:"); + // Serial.println(rem_speed_ref); // Encoders - // Serial.print("m1Raw:"); - // Serial.print(m1Raw); - // Serial.print(" "); - // Serial.print("m2Raw:"); - // Serial.println(m2Raw); + // Serial.print("m1Raw:"); + // Serial.print(m1Raw); + // Serial.print(" "); + // Serial.print("m2Raw:"); + // Serial.println(m2Raw); -// Motors - // Serial.print("SpeedControllerOut:"); - // Serial.print(SC_cont_out); - // Serial.print(" "); - // Serial.print("BalanceOLControllerOut:"); - // Serial.print(OL_cont_out); - // Serial.print(" "); - // Serial.print("BalanceILControllerOut:"); - // Serial.print(IL_cont_out); - // Serial.print(" "); - // Serial.print("TurnControllerOut:"); - // Serial.println(TC_cont_out); - // Serial.print(" "); - // Serial.print("M1_CMD:"); - // Serial.print(M1_Speed_CMD); - // Serial.print(" "); - // Serial.print("M2_CMD:"); - // Serial.println(M2_Speed_CMD); + // Motors + // Serial.print("SpeedControllerOut:"); + // Serial.print(SC_cont_out); + // Serial.print(" "); + // Serial.print("BalanceOLControllerOut:"); + // Serial.print(OL_cont_out); + // Serial.print(" "); + // Serial.print("BalanceILControllerOut:"); + // Serial.print(IL_cont_out); + // Serial.print(" "); + // Serial.print("TurnControllerOut:"); + // Serial.println(TC_cont_out); + // Serial.print(" "); + // Serial.print("M1_CMD:"); + // Serial.print(M1_Speed_CMD); + // Serial.print(" "); + // Serial.print("M2_CMD:"); + // Serial.println(M2_Speed_CMD); - // Serial.print("M1_Ang_Vel:"); - // Serial.print(motor_ang_vel[0][0]); - // Serial.print(" "); - // 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]); + // Serial.print("M1_Ang_Vel:"); + // Serial.print(motor_ang_vel[0][0]); + // Serial.print(" "); + // 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]); // //PS3 Controller // if (Ps3.isConnected()) { From 72716f2abd96a17776a990f4c1a794a8cefd8f8e Mon Sep 17 00:00:00 2001 From: Stedd Date: Thu, 19 Oct 2023 00:26:26 +0200 Subject: [PATCH 03/22] quicksave --- Main/IMU.ino | 26 +++++++++++++++----------- Main/Main.ino | 14 +++++++++----- Main/motorControl.ino | 16 ++++++++++------ Main/plot.ino | 9 ++++++--- README.md | 16 +++++++++++++++- 5 files changed, 55 insertions(+), 26 deletions(-) diff --git a/Main/IMU.ino b/Main/IMU.ino index 83ff38b..e75255d 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -7,7 +7,7 @@ const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? //IMU VARIABLES int ax, ay, az; int cx, cy, cz; -int gx, gy, gz; +float gx, gy, gz; float gt; float acc_pitch; float pitch_rate; @@ -16,26 +16,30 @@ float pitch_prev = 0; void readIMU() { + // Serial.println("ReadingIMU"); //Acceletometer - ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value); - ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value); - az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value); + int* accelerometerReadings = IMU.readFromAccelerometer(); + ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value); + ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value); + az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value); //Magnetometer - cx = IMU.compass_x(IMU.readFromCompass()); - cy = IMU.compass_y(IMU.readFromCompass()); - cz = IMU.compass_z(IMU.readFromCompass()); + int* compassReadings = IMU.readFromCompass(); + cx = IMU.compass_x(compassReadings); + cy = IMU.compass_y(compassReadings); + cz = IMU.compass_z(compassReadings); // Gyrocope - gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate - gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate - gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate + float* gyroReadings = IMU.readGyro(); + gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate //Temperature sensor - gt = IMU.temp(IMU.readGyro()); + gt = IMU.temp(gyroReadings); // Pitch angle from accelerometer diff --git a/Main/Main.ino b/Main/Main.ino index 7a2c05c..adb3cef 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -49,11 +49,14 @@ void setup() { delay(10); //Initialice I2C - Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); - delay(10); + //Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); + //delay(10); //Initialize IMU - IMU.init(); + Serial.println("Before IMU init"); + IMU.init(IMU_I2C_SCL, IMU_I2C_SDA); + //IMU.init(); + Serial.println("After IMU init"); delay(10); //Initialize encoder interrupts @@ -84,6 +87,7 @@ void setup() { } void loop() { + // Serial.println("Loop"); //Update time variables tNow = micros(); dT = tNow - tLast; //[Cycle time in microseconds] @@ -91,7 +95,7 @@ void loop() { //Get sensor data - readIMU(); + //readIMU(); //Control motors @@ -99,7 +103,7 @@ void loop() { // Plot - //plot(); + plot(); //Save time for next cycle diff --git a/Main/motorControl.ino b/Main/motorControl.ino index 60694ab..743be63 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -28,7 +28,7 @@ float SC_cont_out; float TC_cont_out; float OL_cont_out; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; - +float speedCmd1, speedCmd2; //Matrices @@ -84,14 +84,18 @@ void motors() { M2_Speed_CMD = IL_cont_out + TC_cont_out; //Sum speed command for motors - M1_Speed_CMD = 0; - M2_Speed_CMD = 0; + speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0); + M1_Speed_CMD = MOTOR_SATURATION * speedCmd1; + motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0); + M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); //Motor control - IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); - IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); - IL_anti_windup = IL_anti_windup / 2; + // IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + // IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + // IL_anti_windup = IL_anti_windup / 2; //Update variables for next scan cycle m1RawLast = m1Raw; diff --git a/Main/plot.ino b/Main/plot.ino index 5217571..7a9c157 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -9,7 +9,7 @@ void plot() { // IMU // Serial.print ( "Pitch:" ); - // Serial.print ( pitch ); + // Serial.println ( pitch ); // Serial.print (" "); // Serial.print ( "Accelerometer_Pitch:" ); // Serial.print ( acc_pitch ); @@ -52,12 +52,15 @@ void plot() { // Serial.print("BalanceILControllerOut:"); // Serial.print(IL_cont_out); // Serial.print(" "); - // Serial.print("TurnControllerOut:"); - // Serial.println(TC_cont_out); + // Serial.print("SpeedCmd1:"); + // Serial.println(speedCmd1); // Serial.print(" "); // Serial.print("M1_CMD:"); // Serial.print(M1_Speed_CMD); // Serial.print(" "); + // Serial.print("SpeedCmd2:"); + // Serial.println(speedCmd2); + // Serial.print(" "); // Serial.print("M2_CMD:"); // Serial.println(M2_Speed_CMD); diff --git a/README.md b/README.md index f23c65d..640f94a 100644 --- a/README.md +++ b/README.md @@ -1 +1,15 @@ -# Balancebot +# BalanceBot + +## Arduino Board Settings +My board has this etched on it: ESP-WROOM-32 + +These settings allow upload: +* Board: FireBeetle-ESP32 + + +## Dependencies +[Ps3Controller.h](https://github.com/jvpernis/esp32-ps3) + +[MatrixMath.h](https://github.com/eecharlie/MatrixMath) + +[GY_85.h](https://github.com/sqrtmo/GY-85-arduino/tree/master) From 42034ab9e80e97d6d3bdeda9bbc373d0fce37445 Mon Sep 17 00:00:00 2001 From: Stedd Date: Thu, 19 Oct 2023 20:43:01 +0200 Subject: [PATCH 04/22] this is not working --- Main/IMU.ino | 14 +++++++------- Main/Main.ino | 10 +++++----- Main/plot.ino | 4 ++-- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/Main/IMU.ino b/Main/IMU.ino index e75255d..475b5e8 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -31,15 +31,15 @@ void readIMU() { cz = IMU.compass_z(compassReadings); - // Gyrocope - float* gyroReadings = IMU.readGyro(); - gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate - gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate - gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate + // // Gyrocope + // float* gyroReadings = IMU.readGyro(); + // gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate + // gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate + // gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate - //Temperature sensor - gt = IMU.temp(gyroReadings); + // //Temperature sensor + // gt = IMU.temp(gyroReadings); // Pitch angle from accelerometer diff --git a/Main/Main.ino b/Main/Main.ino index adb3cef..66637f6 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -18,8 +18,8 @@ const byte M1_A = 16; const byte M1_B = 17; const byte M2_A = 18; const byte M2_B = 19; -const byte IMU_I2C_SCL = 26; -const byte IMU_I2C_SDA = 27; +const int IMU_I2C_SCL = 26; +const int IMU_I2C_SDA = 27; //Time variables @@ -49,12 +49,12 @@ void setup() { delay(10); //Initialice I2C - //Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); + Wire.setPins(IMU_I2C_SCL, IMU_I2C_SDA); //delay(10); //Initialize IMU Serial.println("Before IMU init"); - IMU.init(IMU_I2C_SCL, IMU_I2C_SDA); + IMU.init(); //IMU.init(); Serial.println("After IMU init"); delay(10); @@ -95,7 +95,7 @@ void loop() { //Get sensor data - //readIMU(); + readIMU(); //Control motors diff --git a/Main/plot.ino b/Main/plot.ino index 7a9c157..64664cf 100644 --- a/Main/plot.ino +++ b/Main/plot.ino @@ -8,8 +8,8 @@ void plot() { // Serial.print(" "); // IMU - // Serial.print ( "Pitch:" ); - // Serial.println ( pitch ); + Serial.print ( "Pitch:" ); + Serial.println ( pitch ); // Serial.print (" "); // Serial.print ( "Accelerometer_Pitch:" ); // Serial.print ( acc_pitch ); From 94c0d8621bb49da41831c887ec78bc7b8daa54f9 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 13:25:29 +0200 Subject: [PATCH 05/22] Renamed Main and placed scripts in root folder --- Main/Main.ino => Balancebot.ino | 0 Main/IMU.ino => IMU.ino | 0 Main/interruptEncoders.ino => interruptEncoders.ino | 0 Main/motorControl.ino => motorControl.ino | 0 Main/plot.ino => plot.ino | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename Main/Main.ino => Balancebot.ino (100%) rename Main/IMU.ino => IMU.ino (100%) rename Main/interruptEncoders.ino => interruptEncoders.ino (100%) rename Main/motorControl.ino => motorControl.ino (100%) rename Main/plot.ino => plot.ino (100%) diff --git a/Main/Main.ino b/Balancebot.ino similarity index 100% rename from Main/Main.ino rename to Balancebot.ino diff --git a/Main/IMU.ino b/IMU.ino similarity index 100% rename from Main/IMU.ino rename to IMU.ino diff --git a/Main/interruptEncoders.ino b/interruptEncoders.ino similarity index 100% rename from Main/interruptEncoders.ino rename to interruptEncoders.ino diff --git a/Main/motorControl.ino b/motorControl.ino similarity index 100% rename from Main/motorControl.ino rename to motorControl.ino diff --git a/Main/plot.ino b/plot.ino similarity index 100% rename from Main/plot.ino rename to plot.ino From 61684e11a62e780d539cdf0f8f1cdeaf42f9214c Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 15:00:22 +0200 Subject: [PATCH 06/22] Code compiles and runs Getting correct pitch angle in SerialPlotter --- Balancebot.ino | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Balancebot.ino b/Balancebot.ino index 66637f6..b894d2c 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -18,8 +18,8 @@ const byte M1_A = 16; const byte M1_B = 17; const byte M2_A = 18; const byte M2_B = 19; -const int IMU_I2C_SCL = 26; -const int IMU_I2C_SDA = 27; +const int IMU_I2C_SDA = 26; +const int IMU_I2C_SCL = 27; //Time variables @@ -49,7 +49,7 @@ void setup() { delay(10); //Initialice I2C - Wire.setPins(IMU_I2C_SCL, IMU_I2C_SDA); + Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); //delay(10); //Initialize IMU From b854a09de0a28a65cab7bccd461e7b702582d3db Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 15:07:32 +0200 Subject: [PATCH 07/22] Changed index of Velocity matrix Compiler gave an error on "index out of bounds", have to test if turn controller is still working after this change. This is the first time i see this error,which leaves me a bit confused as of i see this now. --- motorControl.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorControl.ino b/motorControl.ino index 743be63..1869c87 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -76,7 +76,7 @@ void motors() { //Turn controller - TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC); + TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[1][0], K_TC); //Sum speed command for motors From c58b413392e9aec41eb0d48976a20ec8408b89aa Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 15:09:12 +0200 Subject: [PATCH 08/22] Added board manager and libraries versions to Readme Had a lot of trouble compiling the project after updating packages and libraries after launching the project after 4 years. Hopefully having a log of used libraries makes this task easier in the future --- README.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 640f94a..a5261a7 100644 --- a/README.md +++ b/README.md @@ -8,8 +8,13 @@ These settings allow upload: ## Dependencies -[Ps3Controller.h](https://github.com/jvpernis/esp32-ps3) +### Boards Manager +[esp32 v1.0.4](https://github.com/espressif/arduino-esp32) +#### Libraries +- Wire @1.0.1 +### Libraries +[Ps3Controller.h @v1.1.0](https://github.com/jvpernis/esp32-ps3) -[MatrixMath.h](https://github.com/eecharlie/MatrixMath) +[MatrixMath.h @v1.0](https://github.com/eecharlie/MatrixMath) -[GY_85.h](https://github.com/sqrtmo/GY-85-arduino/tree/master) +[GY_85.h @commit af33c9f791618cec6ae218fe73d039276448ff4f](https://github.com/sqrtmo/GY-85-arduino/tree/master) From 62e07ce32a957920d96ebe754a49232c1c51a044 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 15:55:08 +0200 Subject: [PATCH 09/22] Balance controller is not working --- Balancebot.ino | 7 ++-- IMU.ino | 30 +++++++--------- motorControl.ino | 92 ++++++++++++++++++++++++++++++------------------ plot.ino | 55 ++++++++++++++++------------- 4 files changed, 104 insertions(+), 80 deletions(-) diff --git a/Balancebot.ino b/Balancebot.ino index b894d2c..19f254b 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -45,18 +45,19 @@ const char* _ps3Address = "18:5e:0f:92:00:6c"; void setup() { //Initialize serial - Serial.begin(57600); + Serial.begin(19200); delay(10); //Initialice I2C Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); - //delay(10); + delay(10); + //Initialize IMU Serial.println("Before IMU init"); IMU.init(); - //IMU.init(); Serial.println("After IMU init"); + delay(10); //Initialize encoder interrupts diff --git a/IMU.ino b/IMU.ino index 475b5e8..83ff38b 100644 --- a/IMU.ino +++ b/IMU.ino @@ -7,7 +7,7 @@ const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? //IMU VARIABLES int ax, ay, az; int cx, cy, cz; -float gx, gy, gz; +int gx, gy, gz; float gt; float acc_pitch; float pitch_rate; @@ -16,30 +16,26 @@ float pitch_prev = 0; void readIMU() { - // Serial.println("ReadingIMU"); //Acceletometer - int* accelerometerReadings = IMU.readFromAccelerometer(); - ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value); - ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value); - az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value); + ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value); + ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value); + az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value); //Magnetometer - int* compassReadings = IMU.readFromCompass(); - cx = IMU.compass_x(compassReadings); - cy = IMU.compass_y(compassReadings); - cz = IMU.compass_z(compassReadings); + cx = IMU.compass_x(IMU.readFromCompass()); + cy = IMU.compass_y(IMU.readFromCompass()); + cz = IMU.compass_z(IMU.readFromCompass()); - // // Gyrocope - // float* gyroReadings = IMU.readGyro(); - // gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate - // gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate - // gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate + // Gyrocope + gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate - // //Temperature sensor - // gt = IMU.temp(gyroReadings); + //Temperature sensor + gt = IMU.temp(IMU.readGyro()); // Pitch angle from accelerometer diff --git a/motorControl.ino b/motorControl.ino index 1869c87..75178a6 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -30,6 +30,7 @@ float OL_cont_out; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; float speedCmd1, speedCmd2; +bool balancingOn = false; //Matrices mtx_type motor_ang_vel[2][1]; @@ -47,61 +48,82 @@ void initMotors() { void motors() { + if (Ps3.data.button.cross) { + ResetIntegrators(); + balancingOn = true; + } - //Calculate wheel angular velocity - motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); - motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + if (Ps3.data.button.circle) { + balancingOn = false; + } + + if (Ps3.data.button.triangle) { + ResetIntegrators(); + } + + if (balancingOn) { + + //Calculate wheel angular velocity + motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); - //Calculate robot linear and angular velocity - Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); + //Calculate robot linear and angular velocity + Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); - //Get Control Commands - rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); - rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); + //Get Control Commands + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); - // Speed Controller - SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); + // Speed Controller + SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); - // Balance controller - // Outer loop - 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 + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL)))); - IL_cont_out = round((error_IL * K_IL) + iError_IL); + // Balance controller + // Outer loop + 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 + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL)))); + IL_cont_out = round((error_IL * K_IL) + iError_IL); - //Turn controller - TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[1][0], K_TC); + //Turn controller + 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; + //Motor control + IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + IL_anti_windup = IL_anti_windup / 2; - //Sum speed command for motors - M1_Speed_CMD = IL_cont_out - TC_cont_out; - M2_Speed_CMD = IL_cont_out + TC_cont_out; + } else { - //Sum speed command for motors - speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0); - M1_Speed_CMD = MOTOR_SATURATION * speedCmd1; - motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + //Sum speed command for motors + speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0); + M1_Speed_CMD = MOTOR_SATURATION * speedCmd1; + motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); - speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0); - M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; - motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0); + M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + } - //Motor control - // IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); - // IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); - // IL_anti_windup = IL_anti_windup / 2; //Update variables for next scan cycle m1RawLast = m1Raw; m2RawLast = m2Raw; } +void ResetIntegrators() { + iError_IL = 0.0; + IL_anti_windup = 0.0; +} + float PController(float ref_, float act_, float k_) { return (ref_ - act_) * k_; } diff --git a/plot.ino b/plot.ino index 64664cf..b71dd6d 100644 --- a/plot.ino +++ b/plot.ino @@ -8,18 +8,20 @@ void plot() { // Serial.print(" "); // IMU - Serial.print ( "Pitch:" ); - Serial.println ( 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.println(pitch); + + // Serial.print("Accelerometer_Pitch:"); + // Serial.println(acc_pitch); + + Serial.print("RollRate"); + Serial.println(gz); + + // Serial.print(","); + // Serial.println(gt); + + // Serial.print(" "); + // Serial.println(acc_pitch); // Remote control @@ -43,20 +45,23 @@ void plot() { // Serial.println(m2Raw); // Motors - // Serial.print("SpeedControllerOut:"); - // Serial.print(SC_cont_out); - // Serial.print(" "); - // Serial.print("BalanceOLControllerOut:"); - // Serial.print(OL_cont_out); - // Serial.print(" "); - // Serial.print("BalanceILControllerOut:"); - // Serial.print(IL_cont_out); - // Serial.print(" "); + Serial.print("SpeedControllerOut:"); + Serial.println(SC_cont_out); + + Serial.print("BalanceOLControllerOut:"); + Serial.println(OL_cont_out); + + Serial.print("BalanceILControllerOut:"); + Serial.println(IL_cont_out); + + Serial.print("AntiWindup:"); + Serial.println(IL_anti_windup); + // Serial.print("SpeedCmd1:"); // Serial.println(speedCmd1); // Serial.print(" "); // Serial.print("M1_CMD:"); - // Serial.print(M1_Speed_CMD); + // Serial.println(M1_Speed_CMD); // Serial.print(" "); // Serial.print("SpeedCmd2:"); // Serial.println(speedCmd2); @@ -65,13 +70,13 @@ void plot() { // Serial.println(M2_Speed_CMD); // Serial.print("M1_Ang_Vel:"); - // Serial.print(motor_ang_vel[0][0]); + // Serial.println(motor_ang_vel[0][0]); // Serial.print(" "); // Serial.print("M2_Ang_Vel:"); - // Serial.print(motor_ang_vel[0][1]); + // Serial.println(motor_ang_vel[0][1]); // Serial.print(" "); // Serial.print("botLinVel:"); - // Serial.print(vel_Matrix[0][0]); + // Serial.println(vel_Matrix[0][0]); // Serial.print(" "); // Serial.print("botAngVel:"); // Serial.println(vel_Matrix[1][0]); From b8e6e120d46a3510355634f8d6710a2d9eb65d7b Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 17:51:45 +0200 Subject: [PATCH 10/22] Balance Controller kind of works Not sure why the gains dont work anymore. Anyways, continuing with UDP publishing. --- Balancebot.ino | 2 +- motorControl.ino | 20 +++++++++++------- plot.ino | 53 +++++++++++++++++++++--------------------------- 3 files changed, 37 insertions(+), 38 deletions(-) diff --git a/Balancebot.ino b/Balancebot.ino index 19f254b..06d2a28 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -45,7 +45,7 @@ const char* _ps3Address = "18:5e:0f:92:00:6c"; void setup() { //Initialize serial - Serial.begin(19200); + Serial.begin(9600); delay(10); //Initialice I2C diff --git a/motorControl.ino b/motorControl.ino index 75178a6..59c7a23 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -13,14 +13,14 @@ const float DEADBAND_M2_NEG = 90.0; //Tuning -const float K_SC = 18.5; //Speed controller gain -const float K_TC = 90.0; //Turn controller gain -const float K_OL = 13.0; //Outer loop balance controller gain -const float K_IL = 72.0; //Inner loop balance controller gain -const float I_IL = 80.0; //Inner loop balance controller Igain +const float gainScale = 0.75; +const float K_SC = 18.5*gainScale; //Speed controller gain +const float K_TC = 90.0*gainScale; //Turn controller gain +const float K_OL = 13.0*gainScale; //Outer loop balance controller gain +const float K_IL = 72.0*gainScale; //Inner loop balance controller gain +const float I_IL = 80.0*gainScale; //Inner loop balance controller Igain const float filter_gain = 16.0; //Motor speed LPF gain - //Help variables int M1_Speed_CMD, M2_Speed_CMD; float rem_speed_ref, rem_turn_speed_ref; @@ -30,7 +30,7 @@ float OL_cont_out; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; float speedCmd1, speedCmd2; -bool balancingOn = false; +bool balancingOn = true; //Matrices mtx_type motor_ang_vel[2][1]; @@ -61,6 +61,10 @@ void motors() { ResetIntegrators(); } + if (Ps3.data.button.square) { + IMU.init(); + } + if (balancingOn) { //Calculate wheel angular velocity @@ -92,6 +96,7 @@ void motors() { //Turn controller 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; @@ -173,6 +178,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl speedCMD_ = speedCMD_; } + //Apply speed command to PWM output if (speedCMD_ > 0) { ledcWrite(ch1, 0); diff --git a/plot.ino b/plot.ino index b71dd6d..a86201b 100644 --- a/plot.ino +++ b/plot.ino @@ -8,20 +8,19 @@ void plot() { // Serial.print(" "); // IMU - Serial.print("Pitch:"); - Serial.println(pitch); + + // Serial.print("RollRate:"); + // Serial.println(pitch_rate); // Serial.print("Accelerometer_Pitch:"); // Serial.println(acc_pitch); - Serial.print("RollRate"); - Serial.println(gz); - - // Serial.print(","); - // Serial.println(gt); - - // Serial.print(" "); - // Serial.println(acc_pitch); + // Serial.print("Pitch:"); + // Serial.println(pitch); + // Serial.print ( "," ); + // Serial.println ( gt ); + // Serial.print ( " " ); + // Serial.println ( acc_pitch); // Remote control @@ -39,41 +38,35 @@ void plot() { // Encoders // Serial.print("m1Raw:"); - // Serial.print(m1Raw); - // Serial.print(" "); + // Serial.println(m1Raw); + // Serial.print("m2Raw:"); // Serial.println(m2Raw); - // Motors - Serial.print("SpeedControllerOut:"); - Serial.println(SC_cont_out); + // // Motors + // Serial.print("SpeedControllerOut:"); + // Serial.println(SC_cont_out); - Serial.print("BalanceOLControllerOut:"); - Serial.println(OL_cont_out); + // Serial.print("BalanceOLControllerOut:"); + // Serial.println(OL_cont_out); - Serial.print("BalanceILControllerOut:"); - Serial.println(IL_cont_out); + // Serial.print("BalanceILControllerOut:"); + // Serial.println(IL_cont_out); - Serial.print("AntiWindup:"); - Serial.println(IL_anti_windup); + // Serial.print("TurnControllerOut:"); + // Serial.println(TC_cont_out); - // Serial.print("SpeedCmd1:"); - // Serial.println(speedCmd1); - // Serial.print(" "); // Serial.print("M1_CMD:"); // Serial.println(M1_Speed_CMD); - // Serial.print(" "); - // Serial.print("SpeedCmd2:"); - // Serial.println(speedCmd2); - // Serial.print(" "); + // Serial.print("M2_CMD:"); // Serial.println(M2_Speed_CMD); // Serial.print("M1_Ang_Vel:"); - // Serial.println(motor_ang_vel[0][0]); + // Serial.print(motor_ang_vel[0][0]); // Serial.print(" "); // Serial.print("M2_Ang_Vel:"); - // Serial.println(motor_ang_vel[0][1]); + // Serial.print(motor_ang_vel[0][1]); // Serial.print(" "); // Serial.print("botLinVel:"); // Serial.println(vel_Matrix[0][0]); From c49c511d75bb3a3df105d4bdbd8aa1dc4905c89d Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 18:01:53 +0200 Subject: [PATCH 11/22] First UDP test Copied code from UDP example Added calls in setup and loop Added Wifi config --- Balancebot.ino | 8 +++++--- UDP.ino | 52 ++++++++++++++++++++++++++++++++++++++++++++++++ motorControl.ino | 2 +- 3 files changed, 58 insertions(+), 4 deletions(-) create mode 100644 UDP.ino diff --git a/Balancebot.ino b/Balancebot.ino index 06d2a28..ee1d424 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -85,6 +85,9 @@ void setup() { //Initialize PS3 controller connection Ps3.begin(_ps3Address); + + //Init UDP + UdpInit(); } void loop() { @@ -106,13 +109,12 @@ void loop() { // Plot plot(); + //Udp + UdpLoop(); //Save time for next cycle tLast = tNow; - //Delay delay(5); - - //Test } diff --git a/UDP.ino b/UDP.ino new file mode 100644 index 0000000..4aed47f --- /dev/null +++ b/UDP.ino @@ -0,0 +1,52 @@ +#include "WiFi.h" +#include "AsyncUDP.h" + +const char * ssid = "CaveBot"; +const char * password = "Eating0-Untaxed0-Pod6-Jokester8"; + +AsyncUDP udp; + +void UdpInit() +{ + //Serial.begin(115200); + WiFi.mode(WIFI_STA); + WiFi.begin(ssid, password); + if (WiFi.waitForConnectResult() != WL_CONNECTED) { + Serial.println("WiFi Failed"); + while(1) { + delay(1000); + } + } + if(udp.listenMulticast(IPAddress(239,1,2,3), 1234)) { + Serial.print("UDP Listening on IP: "); + Serial.println(WiFi.localIP()); + udp.onPacket([](AsyncUDPPacket packet) { + Serial.print("UDP Packet Type: "); + Serial.print(packet.isBroadcast()?"Broadcast":packet.isMulticast()?"Multicast":"Unicast"); + Serial.print(", From: "); + Serial.print(packet.remoteIP()); + Serial.print(":"); + Serial.print(packet.remotePort()); + Serial.print(", To: "); + Serial.print(packet.localIP()); + Serial.print(":"); + Serial.print(packet.localPort()); + Serial.print(", Length: "); + Serial.print(packet.length()); + Serial.print(", Data: "); + Serial.write(packet.data(), packet.length()); + Serial.println(); + //reply to the client + packet.printf("Got %u bytes of data", packet.length()); + }); + //Send multicast + udp.print("Hello!"); + } +} + +void UdpLoop() +{ + delay(1000); + //Send multicast + udp.print("Anyone here?"); +} diff --git a/motorControl.ino b/motorControl.ino index 59c7a23..69558a0 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -30,7 +30,7 @@ float OL_cont_out; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; float speedCmd1, speedCmd2; -bool balancingOn = true; +bool balancingOn = false; //Matrices mtx_type motor_ang_vel[2][1]; From 0991343a55642c162b7694b027f8db3e68ca6b1e Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 19:19:43 +0200 Subject: [PATCH 12/22] UDP Packets appear in wireshark and UDPlistener --- UDP.ino | 67 +++++++++++++++++++++------------------------------------ 1 file changed, 24 insertions(+), 43 deletions(-) diff --git a/UDP.ino b/UDP.ino index 4aed47f..01960c3 100644 --- a/UDP.ino +++ b/UDP.ino @@ -1,52 +1,33 @@ +const char* ssid = "CaveBot"; +const char* password = "&nHM%D2!$]Qg[VUv"; + #include "WiFi.h" #include "AsyncUDP.h" -const char * ssid = "CaveBot"; -const char * password = "Eating0-Untaxed0-Pod6-Jokester8"; +const IPAddress multicastIP = IPAddress(239, 1, 2, 3); +int port = 1234; AsyncUDP udp; -void UdpInit() -{ - //Serial.begin(115200); - WiFi.mode(WIFI_STA); - WiFi.begin(ssid, password); - if (WiFi.waitForConnectResult() != WL_CONNECTED) { - Serial.println("WiFi Failed"); - while(1) { - delay(1000); - } - } - if(udp.listenMulticast(IPAddress(239,1,2,3), 1234)) { - Serial.print("UDP Listening on IP: "); - Serial.println(WiFi.localIP()); - udp.onPacket([](AsyncUDPPacket packet) { - Serial.print("UDP Packet Type: "); - Serial.print(packet.isBroadcast()?"Broadcast":packet.isMulticast()?"Multicast":"Unicast"); - Serial.print(", From: "); - Serial.print(packet.remoteIP()); - Serial.print(":"); - Serial.print(packet.remotePort()); - Serial.print(", To: "); - Serial.print(packet.localIP()); - Serial.print(":"); - Serial.print(packet.localPort()); - Serial.print(", Length: "); - Serial.print(packet.length()); - Serial.print(", Data: "); - Serial.write(packet.data(), packet.length()); - Serial.println(); - //reply to the client - packet.printf("Got %u bytes of data", packet.length()); - }); - //Send multicast - udp.print("Hello!"); - } +void UdpInit() { + //Serial.begin(115200); + ConnectToWiFi(); + //udp.connect(multicastIP, port); } -void UdpLoop() -{ - delay(1000); - //Send multicast - udp.print("Anyone here?"); +void UdpLoop() { + byte data[8] = {1, 2, 3, 4, 5, 6, 7, 8}; + udp.writeTo(data, sizeof(data), multicastIP, port); +} + + +void ConnectToWiFi() { + WiFi.mode(WIFI_STA); + WiFi.begin(ssid, password); + if (WiFi.waitForConnectResult() != WL_CONNECTED) { + Serial.println("WiFi Failed"); + while (1) { + delay(1000); + } + } } From 7ab1ee525dd38d3a04b049300a672b6d11f97378 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 19:37:38 +0200 Subject: [PATCH 13/22] Watchdog in the first Byte --- UDP.ino | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/UDP.ino b/UDP.ino index 01960c3..3c984bb 100644 --- a/UDP.ino +++ b/UDP.ino @@ -7,6 +7,8 @@ const char* password = "&nHM%D2!$]Qg[VUv"; const IPAddress multicastIP = IPAddress(239, 1, 2, 3); int port = 1234; +byte watchdog = 0; + AsyncUDP udp; void UdpInit() { @@ -16,8 +18,9 @@ void UdpInit() { } void UdpLoop() { - byte data[8] = {1, 2, 3, 4, 5, 6, 7, 8}; - udp.writeTo(data, sizeof(data), multicastIP, port); + byte data[8] = { 1, 2, 3, 4, 5, 6, 7, 8 }; + data[0] = watchdog++; + udp.writeTo(data, sizeof(data), multicastIP, port); } From 6a03848435ac16248a6f124fbc1e39a0ce6b7328 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 01:18:13 +0200 Subject: [PATCH 14/22] Packing data for UDP telegram Naive approach, but it works for now --- Balancebot.ino | 4 +++- UDP.ino | 3 --- plot.ino | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 52 insertions(+), 4 deletions(-) diff --git a/Balancebot.ino b/Balancebot.ino index ee1d424..fae01b6 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -43,6 +43,9 @@ volatile bool M2_A_state, M2_B_state; //PS3 Controller variables const char* _ps3Address = "18:5e:0f:92:00:6c"; +//UDP variables +uint8_t data[30 * 4]; + void setup() { //Initialize serial Serial.begin(9600); @@ -105,7 +108,6 @@ void loop() { //Control motors motors(); - // Plot plot(); diff --git a/UDP.ino b/UDP.ino index 3c984bb..19e6b33 100644 --- a/UDP.ino +++ b/UDP.ino @@ -8,7 +8,6 @@ const IPAddress multicastIP = IPAddress(239, 1, 2, 3); int port = 1234; byte watchdog = 0; - AsyncUDP udp; void UdpInit() { @@ -18,8 +17,6 @@ void UdpInit() { } void UdpLoop() { - byte data[8] = { 1, 2, 3, 4, 5, 6, 7, 8 }; - data[0] = watchdog++; udp.writeTo(data, sizeof(data), multicastIP, port); } diff --git a/plot.ino b/plot.ino index a86201b..bc73582 100644 --- a/plot.ino +++ b/plot.ino @@ -88,4 +88,53 @@ void plot() { // Serial.print("ry:"); // Serial.println(Ps3.data.analog.stick.ry); // } + + int i = 0; + data[i] = watchdog++; + data[i += 1] = balancingOn<<1; + i = PackInt(i+=1, M1_Speed_CMD); + i = PackInt(i, M2_Speed_CMD); + i = PackFloat(i, acc_pitch); + i = PackFloat(i, pitch); + i = PackFloat(i, pitch_rate); + i = PackFloat(i, rem_speed_ref); + i = PackFloat(i, rem_turn_speed_ref); + i = PackFloat(i, SC_cont_out); + i = PackFloat(i, TC_cont_out); + i = PackFloat(i, OL_cont_out); + i = PackFloat(i, ref_IL); + i = PackFloat(i, error_IL); + i = PackFloat(i, IL_cont_out); + i = PackFloat(i, iError_IL); + i = PackFloat(i, IL_anti_windup); + i = PackFloat(i, speedCmd1); + i = PackFloat(i, speedCmd2); } + +int PackInt(int _i, int value) { + data[_i] = (value & 0x00FF) ; + data[_i + 1] = (value & 0xFF00)>>8; + return _i + 2; +} + +union FloatToBytes { + float value; + byte bytes[4]; +}; + +int PackFloat(int _i, float value) { + FloatToBytes converter; + converter.value = value; + for(int j = 0; j < 4; j++) { + data[_i + j] = converter.bytes[j]; + } + return _i + 4; +} + +// int PackFloat(int _i, float value) { +// data[_i] = (value & 0x000000FF) ; +// data[_i + 2] = (value & 0x0000FF00)>>8; +// data[_i + 3] = (value & 0x00FF0000)>>16; +// data[_i + 4] = (value & 0xFF000000)>>24; +// return _i + 4; +// } From 3a680f84c85fcf9f2f879c25f9792dca0a3461bf Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 02:06:40 +0200 Subject: [PATCH 15/22] Fixed a bug in LeftMotorAngular velocity calculation --- motorControl.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorControl.ino b/motorControl.ino index 69558a0..b773f36 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -68,7 +68,7 @@ void motors() { if (balancingOn) { //Calculate wheel angular velocity - motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); From 141a03cb840ba6f2e4ffe2821029f6a1f4bea688 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 02:07:22 +0200 Subject: [PATCH 16/22] Bot can balance by itself again --- motorControl.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motorControl.ino b/motorControl.ino index b773f36..9220e02 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -13,13 +13,13 @@ const float DEADBAND_M2_NEG = 90.0; //Tuning -const float gainScale = 0.75; const float K_SC = 18.5*gainScale; //Speed controller gain const float K_TC = 90.0*gainScale; //Turn controller gain const float K_OL = 13.0*gainScale; //Outer loop balance controller gain const float K_IL = 72.0*gainScale; //Inner loop balance controller gain const float I_IL = 80.0*gainScale; //Inner loop balance controller Igain const float filter_gain = 16.0; //Motor speed LPF gain +const float gainScale = 1; //Help variables int M1_Speed_CMD, M2_Speed_CMD; From 3343c2ac981d8deb4e8c547db618da9c56b5d1f2 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 02:08:19 +0200 Subject: [PATCH 17/22] Added some more variables to UDP --- plot.ino | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/plot.ino b/plot.ino index bc73582..2587668 100644 --- a/plot.ino +++ b/plot.ino @@ -91,8 +91,8 @@ void plot() { int i = 0; data[i] = watchdog++; - data[i += 1] = balancingOn<<1; - i = PackInt(i+=1, M1_Speed_CMD); + data[i += 1] = balancingOn << 1; + i = PackInt(i += 1, M1_Speed_CMD); i = PackInt(i, M2_Speed_CMD); i = PackFloat(i, acc_pitch); i = PackFloat(i, pitch); @@ -103,12 +103,19 @@ void plot() { i = PackFloat(i, TC_cont_out); i = PackFloat(i, OL_cont_out); i = PackFloat(i, ref_IL); + i = PackFloat(i, act_IL); i = PackFloat(i, error_IL); i = PackFloat(i, IL_cont_out); i = PackFloat(i, iError_IL); i = PackFloat(i, IL_anti_windup); i = PackFloat(i, speedCmd1); i = PackFloat(i, speedCmd2); + i = PackFloat(i, vel_Matrix[0][0]); + i = PackFloat(i, vel_Matrix[1][0]); + i = PackFloat(i, motor_ang_vel[0][0]); + i = PackFloat(i, motor_ang_vel[1][0]); + i = PackLong(i, m1Raw); + i = PackLong(i, m2Raw); } int PackInt(int _i, int value) { From 1fa5f97b2511fa8a59730e0ad67d65a2cf2fa5b0 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 02:09:38 +0200 Subject: [PATCH 18/22] Moved velocity calculations out of balancing loop so they always can be monitored --- motorControl.ino | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/motorControl.ino b/motorControl.ino index 9220e02..ac66cee 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -65,19 +65,19 @@ void motors() { IMU.init(); } - if (balancingOn) { //Calculate wheel angular velocity motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + //Calculate robot linear and angular velocity + Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); + //Get Control Commands + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); - //Calculate robot linear and angular velocity - Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); + if (balancingOn) { - //Get Control Commands - rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); - rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); // Speed Controller SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); From 26955c1c123894b9cb03a061a6278128b25e03bc Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 02:09:46 +0200 Subject: [PATCH 19/22] Formatting --- motorControl.ino | 15 +++++++-------- plot.ino | 30 ++++++++++++++++++++---------- 2 files changed, 27 insertions(+), 18 deletions(-) diff --git a/motorControl.ino b/motorControl.ino index ac66cee..6b24aca 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -13,13 +13,13 @@ const float DEADBAND_M2_NEG = 90.0; //Tuning -const float K_SC = 18.5*gainScale; //Speed controller gain -const float K_TC = 90.0*gainScale; //Turn controller gain -const float K_OL = 13.0*gainScale; //Outer loop balance controller gain -const float K_IL = 72.0*gainScale; //Inner loop balance controller gain -const float I_IL = 80.0*gainScale; //Inner loop balance controller Igain -const float filter_gain = 16.0; //Motor speed LPF gain const float gainScale = 1; +const float K_SC = 18.5 * gainScale; //Speed controller gain +const float K_TC = 90.0 * gainScale; //Turn controller gain +const float K_OL = 13.0 * gainScale; //Outer loop balance controller gain +const float K_IL = 72.0 * gainScale; //Inner loop balance controller gain +const float I_IL = 80.0 * gainScale; //Inner loop balance controller Igain +const float filter_gain = 16.0; //Motor speed LPF gain //Help variables int M1_Speed_CMD, M2_Speed_CMD; @@ -62,7 +62,7 @@ void motors() { } if (Ps3.data.button.square) { - IMU.init(); + IMU.init(); } @@ -82,7 +82,6 @@ void motors() { // Speed Controller SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); - // Balance controller // Outer loop OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); diff --git a/plot.ino b/plot.ino index 2587668..a1d051f 100644 --- a/plot.ino +++ b/plot.ino @@ -119,25 +119,35 @@ void plot() { } int PackInt(int _i, int value) { - data[_i] = (value & 0x00FF) ; - data[_i + 1] = (value & 0xFF00)>>8; + data[_i] = (value & 0x00FF); + data[_i + 1] = (value & 0xFF00) >> 8; return _i + 2; } +int PackLong(int _i, long value) { + data[_i] = (value & 0x000000FF); + data[_i + 1] = (value & 0x0000FF00) >> 8; + data[_i + 2] = (value & 0x00FF0000) >> 16; + data[_i + 3] = (value & 0xFF000000) >> 24; + return _i + 4; +} + union FloatToBytes { - float value; - byte bytes[4]; + float value; + byte bytes[4]; }; int PackFloat(int _i, float value) { - FloatToBytes converter; - converter.value = value; - for(int j = 0; j < 4; j++) { - data[_i + j] = converter.bytes[j]; - } - return _i + 4; + FloatToBytes converter; + converter.value = value; + for (int j = 0; j < 4; j++) { + data[_i + j] = converter.bytes[j]; + } + return _i + 4; } + + // int PackFloat(int _i, float value) { // data[_i] = (value & 0x000000FF) ; // data[_i + 2] = (value & 0x0000FF00)>>8; From 800fb1876e513f474b544ef103ec0e2c8adae761 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 12:04:25 +0200 Subject: [PATCH 20/22] Fixed bug in motor windup calculation --- Spreadsheets/MotorSaturation_Bug.ods | Bin 0 -> 16493 bytes motorControl.ino | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 Spreadsheets/MotorSaturation_Bug.ods diff --git a/Spreadsheets/MotorSaturation_Bug.ods b/Spreadsheets/MotorSaturation_Bug.ods new file mode 100644 index 0000000000000000000000000000000000000000..e256e3a2597025a06c8713c4fedaf07d069d7443 GIT binary patch literal 16493 zcmb7r1ymhd(k|{09D=(B2u^T!cXxMp2<`-Tw}ZR8OK^85xVyt6^S?WHX8wEUtygFD zI=g#yeY>i-0J$ttS^T7CtanvUa3(u(dbRGjuRDGIEstH_Q*1e+t$| zOTfn3*v!Pq{vT)#40N_OwobMm5KeS|uZQ^;dcRfl_3ZzP7S3O2+1lHf*c&-G{CCa2 z>d(=}#`3>w{grnPj(Uzx|Bv4A|H4i)D?Jk<2ReQ;M=L#BhyUW_UzuubX`|<8^xs1B zSEl}U&{ogd$nwAR_E#a*GcYi+H2S!nHh=U81qJnw+v&rve+t}3%SzAM%-G1mk=EYO zcr0$r+W#wJz&U52(fA@S9XPl>#c;EERu^-SDmdL$D)M^Ak2qzy{F0-r*_{eu#*H6! zX{UNPL*X?=-HJ>Gl%&I=ruB!^?5(uT1uN{!iw)HHskoQs{smc7EiTqn-*gfKX;|Sb zCu(ykTvM+(W!i|k=h*B-;Rfn{Ktbb0@|)fM^2GW!z71`xBIkIV9qo2cz!kp6UYuP6 zqt_tc9^60>JLhzJP`7=?KbKSYg8IkA;(7=iU{`yy zz3NQUf5S7u)cwrB-D#%(a)UTtH=W^3=BSm!?S21^cH;f>c4eKjpe65kp2<`6e%%$m zl1Qbj1SC}5z3DVKFc6SGC=k&9JOO_dKA4a2IXJpm8adFqT3N2CTiL9zAij0z5EgY+ z0V|NKSYddC`nNmLOd1QM=`R-g0Vq;1NnuE=$X{ z{=`se{CqtxC^~#C@!b*q5$`8`lv$?s6TXc$`2O46OB!q@32L0$=p_|#GfaMqfuK7< z`iPn^YRioZQI;J^ar!O=u|41-0)xbbsFudrIFYhaqF^ns8gK{^VUn+5*5~p4^U^%jagc@U%ouPoBKd=v^AVf|eU51<>`Q}SEG)HH&`lW?l?@0vQn%w=n;u9iMuwf( z7k8bOk+|Cg^{cKSG?UN+kD%YeWuyIIl`bRDd-DvtZtJg<8VK~is)^>b^9od0yT>^Z z7ZK$`47S@wV6G?bT*!l^4ZzDkg_~kw$)Yi+I;B+;mAgyysC;{Xe;uljv4$lo%H$sn z8N@DbcXmv@ zzQC1C{l4D5b|(-651%u@B8~*B4o91Zyr!FFVg=wo5g{Z1shOv#G|!bts6HbrF~Ma* zvs;A$pNm7pqKX%*0vKMG4bxJRmq=~zc@^!0tO1t}Xh0D;$;LNBBRF)@D#YpvXtkzD zzcjZxkl~!ALidn_KpWe8%OyH7pCl}}_e;Ff1mfv2dEgW1A#eE4?^k^`a!Gn?EZV#( z)YAQSM?b?Hhp}`>^R&lvA&!I+K{oa?a3M^znt%@jaFuUqLagHN}0r7QR2VQh}J7QP@a` z`a87XoLo2bM%8>_^~OzlT@_vjzYpn#^d~!Y$;Qf>Dnb3hi#p zerftJ(P6g0VnNwBMxXy^^!LKg;OTPV@D91(@jf4v`Stn`vcBQN93_5`>E65-hLgX7 zsdT9%c2d>A`>>&S;N`suCAsN;$r47f`%S;w4_w^T+P;RhP`4#@M!h-Y_K=qS8Hs8< zE`4gz!x`@|>r7h*cDv8>&JPopKBSUO)KRl}tJw z_7hDMjTNp4yu^wgd8EkXozLrO+%(H!ft9`avi|kwvmp85M83GE{%hI|Ty)qovpQs8`0o#MT?6(a0Mn(Ws>$EQH-)foSXL4rZSZ1fd#)gC18 z`=wZ1iOmD>@2S3%pGp(wdDujb3ImKWLs5+}#co_3WK|s<9J>>yZ47gdg3^FXpJ<@fSwK($7)vEMQL{~}W>HMmLKzX9mOp-4+ zQCU$z+q1^UA%7BGh2n-gu-AS+ErS>r>B66yW`5gQM|a1crp29;6}9SKU(JP*UIKC0 zP8Q(7xh`_*@thHjXFTXJnyBKJAD>kGDqAz+Yx}14$RmVSDll?^iW&LfSx5dZJiaAUA7-V-b#-84?@~!IC!tMI~V@897YIc7WDuDn3nkWBnGu%J& zO(RFW-}75!+(>NNS45%vM;P&+=r|xRN&`TwDMar)t z|7rnyd4#4=_Q(nOsN){$HKO4$<_>H;n9ACJ?dFulVdcG%7MW-iT6koxbaljWTal(I zu79u@J7|z98<9&oITq-kSlQ^2QeOcGE$}Cc_~F{dRrFbp6F_2YDN?-IoXxrQnS4fJ zi$ky=-!+(luZh4+KH5`AWn~pD9${zOV6OF0>-JvjDItj`5&eAFhVoIwdGq)N|DI@K z9UU&B%IOxP-Q~gx3uhl%Y&yW}sbCNs;q6k>GB@pM=jW11w4S5~0i~~0W;K68-O#Ra ztqCXH9HBp%lvrjB!$d72GbMG=J3tX^ojHyNu2@DAu$NfT7KHe8(?^o%bl$v*-$tzRg}XvMm^aqbVgQP(hVZwJoSXwahs}XVPOaZgSGnJ!vP@ ztIBHgb*|fPEQdoru$22>_-Ow0bS5LJbqsJZti=Zqaglmbg^a|H=|bmTbT65M=gzHZ z88ilCyY^;7vtMSNu9_&wIwI_GK_5j$^Mc$Au++X7(RJ7>3~lCu2i-|B*M`vSIYF-$ z@1a$Dy5g8LYTceRXpBl^u?{C@u`;jiTN28#(D{cqi}hi#f44?N)$6%(4w5-)C-Dz= z3beZj2(dTdgV1qe!Y^$d=x!m#6UawAV+!J!bU~Pr^P;ksnwQIClF2O~^uMCrsmBxk zvh>SVZHMhw$G7O|rzvj!J`}OZq$On$()dBUJQxS_kmN7xgXE&3go+efP@;HOwLha5 zwAJ7S2(r6&NJ!b#(d;q8VF^~|qxZ%iMp1U7CbAGXNqo&Q!WQ$24-o0(Glj)&sE3lvvL}J<>b|xZB#jC z(}QR|w%A@pxMX}Z_xnXp>K=eWAf>AeRn2UK22%&;f_uqAK3T7 z(7E!tBtevd3>AssfO}7~i=sR`efK*#BfbY90nSH!e@gmS^>40Bb}2aMFmL&K4pn`Y+pCc6^jNL#Iq$Wd%2peBvKdBHOPU#zO}G^ zeqL)3UEyISjr4qb5ceG7xUQ4G2_&euT_{w@CWJ4^;Djl>K~{n$8V)H&U4;lSen&oh zN+KC8N4qU3$jMc#C&*&lK$DQu2<+AC7g9gLRj*ilEbws2UAf3mYTC9!))}EbVbteL z4tLgscSpl{8s`02=!|o0q6(h=UwxVv94c?vSu=ubsg8CiSXjsN00&danFms2pN>Xo z;C_0fM#6=4ZtC=GsO}eK6P9~LcR~zERxsfe`5hc{G+F_-!XSJEf+bhNiz#(ke%Qw% z>tap?m$N3=P-43&a&8<+amGq4Gl;FT-B zx+Ss_`~gA)Y`;Yb#jqHs1uY$W9fl0|&TB^pEPA)m;4QK}&R?WlKS2@j2O=(5Uaz9~ z0g{MY0eJiE;*FDN_j}sVyNtj{#dkyE%B!^f9?IHA<6x!wZ0>py2JWk|6I5~{r%D(p zjaWXj{##SbGlaZM8tBnX+Of&?I*E7BceYmq*bEX67rRZI-@eY&?Ko2~$qD!+f(Wdwe> zfPQ4kx)zHdfZ)T{1_3suv*rrbkW`Z6m~g=r)pTbImq6Z~VEWp=hygP8#R17{yYGNP z$DK?QiK^0LrZy8S8Jeh6)SM#8Oe+4zb}6ib}1W|1gKM@ch# zz8_!y__WyQit#NBq<>DK*jig#X>41Oy;TMF6|+Es*eGCP_>$szJ)bBtiB3Luuo8*? zEQFETC&$AkU*tT}{&oQcOfCJ(rj3e0`%&M>;Tt*NrR3nV7nHR5z#XuMb+^tWgf{<$ zBrSV^o_B&>=N-M_Af^5*2KjLUO>EyjJP|P+GdNZK7+@qYmI=vq>@d0#YP*!SLzySy z(*5@0GKuH?elNIFy))8#wd$Vu?Rmqy%6sJdXJ5J47w;#GTNa?}T}Qwy6vNeN`Q*{q z{(6_&Xfb>nBU}rLlvI!*jAMQ2lizRw6=jkMNCDN4aWLA}&zcQnO~%8UsaklN&ah2P zV@?J?7SdATexe?jtK5QXx|D3T9FsR0k8P%!`Hd(;Bzd{nj;VJQm{njqw5`t?iUN35 zT1)6QTa>^Tta!SaZ6FpjQ8W`7n#xA}E!IJ3r5x6hcWwb>0u10S%Sv9r3;bxB;og20 zQ*af5WKEJrwkb)F3%TH$se3K-gUA205~QM;s%h^PY{81L$(p&lV6=S1;J5)?BXz!o zzW2}5{K`|P8jN52=Cv3b&e$1snYfsNEp-3vEwLQtOKVn_It!@9QVMY1VKKBSC?oUd zSjx${iX+&nFG{*OJp|ACf{o+N`)Qa$L0-H z8-@>#epIe!OFWJUPR`h%j3q|?Mxgi$1XgR`%(GPnjfUa0wWYkW4->WiuZb-mCbAyL zuTy<;Z&6vdT>A3SN7g_3Fk4nH;Vv+BODsgqFt@gkZKiz<6bNi)7^rKN9$O7vclT%5 z^??^U2E1qT?Rj~3Z zc-w*<9L5okCc@u+Sj+ZdEm$gmW=0Vr)eStm-l}1texaP)TXB&ZRB9%xrd?s}7Q%Em z>H>tzD=2BM=6(HWeB4A!CGwNxT%D2nFmEmGr#c9axVRIi<0T8UyWvlAb2o*S=ZY@v zz^PZgihMFz0Q35jk;ax>$6KbGxtM45Z?bcl2jXm0*O;g|E|F3=D zqq*5u;%+J<>l#E>TucV*J_4R^w#<;Qp`n|(-w$56oR)4_X<$QQ`hBt;z+>c5*wP9% zUFM$8tWr1&dObWp+3qOsrG*9|~ zVXl*`xZm)9C$>)k!qM3u%Wlabf4sH)u}W^n;d~p82L$wc`@Q_CXzFC8Z>?u$=|JcB zN0Zjp+9Xs~S_B>j>-W+fyqKt<{KtRY$Mz2Oktze(M8H)70f9WpN-7FrVq%hzkWf=o zGcq!AaBy&Qi}DCcbBQPl2ng`=ONfX|$_PnGNJ#LD$O(!o2ui9-NGgacsH;fu%gU%J z$w@0JDk>z6t7(|2>zQj9IT@*n8)>Q9YN+Y! z7#iwondxab7^)Z;=<6F=7@1j`8ylFJSX-JISy)(@SUQ>8x!GHq+1c1R+FE(q8hY4U zIDCkcyQ7I5bv!|!0j&*>gYp|tvw40~Dx1YDQU!1#dsDpp3Q%HtaNSs$# zYM`@afTy#Mw@;{_M|^-oT##ErsAqDpOKPNNQH-~rpI<;wXkbWKSdd>xa7aXGU}R)u zU__ixL|S-cTv$|cU_xeOOnhWqYFtE6bX0V5On6*eTuee*VsdOkN=8~jY;sa^T1rA{ zYHDCyeo$gbaB4+(VqQc_URYXrct&+XdR}Hu){pF()SRNE!p6M#pq%)y{N#v|c)!xL z$h`E#s*F0LxbuWBo+t}Cl+EpMu;D+e^x7S>gjc2wuL)|J-Q*&c8OVcC0Q)9y`<6ZMp1GAH3D^q<-Go!1^<1;fe)AK9S%d>M!E6a0J zt4nihYimCjel4wSZEUVCZ0*l&9<6QtTHQHbKRsAFyx*B0++JJQ-JbuowR*TUcl2xJ z^kD5`XKrU_XM68(_u%AkZ|CH2_vq;8@bq%;>gDv}=IrL_ z?jP^(9-klXu3w&S-rnARKM3#d?#k_6KtR}%VuE~%uFI!fR*KjQ35&+9`9>>_8U@%` zDCAb6h8&3m+9OTMmeO`>MdA)M41GH_;YyJaqrV*EVwI-L>T{%zfa&*i1vFEDIk}er zF$Q}3pci4?T@)ZVvAE#T3FwW+N`>17n$1CSBx6|_NSm01-|+U#TDgLs*VAOy+>%Bz zTsGRzJ6O#Q%`@B&)nbaHeaf{-6)vI^a*+M&`7;1}PeaG6Ai8#E4g|d8xRYPG#Gg=J z{P6a;@m0amFWlR~6lG%Lt}*D~1@(;(gn3$fUmJ85jcv`2&bidy>v?Eh(QDtX&GW+c z&8U}ZtJPg1c%Eu^%CQ?OnHy_QP7%v{+URr7ne@MbI$#LQ*woR_T=TfOqqt989<2UM zd*_z9$?FUWYNo?5>?~KbNtQ1`t7x73K7o>%R(Tp%%1qu_f5vzF)Vm?i-K4~ruUI_# zsif}iq)qBt@SJj1+Rk!qxqOnPy#8u*Tq@0RxXrU*{VGyG25*TFR@prwTz_BPO~wdF%+-zFC6<}U)(Rl z?^$_ZHr`7Zp0$L>?asNzn@6pZ@ao?@P4i^bJTCHRGJ9mq9Xi$BLbNS9CQDlh@wA+d zS=hM-;E!5#wiC2CvMleFjP_MByI>|qL|oL_HF%;M}iTe{8CVR_#1b)IjwRnA>9*xuv4H?T^{BG`-dj67B=#q#TjQJC)NjZE2FQB4KCkr-3d5Y)m< zBh_Efw_LKWNJlZY*!pY)UbwL`d7y&>kEdi$8f3u)VF3H%V1{^HIL~TKSRCP_JOs`S zuDukpsgRocM$Hc-isQojcsQY2spWoB`_syXA7cE)ha(o}J0&q5!;A`GG1R6Vd)-T>}7vi!{G+qExNS>8u8EKRfg9iNEKB& zFjJ%Je$@3QYr{vi6(`g0c4ceNBG2w|Y|^Zpwz=_5s!y`-*DUZ$tPC6OeMq?L+pwL^ zx_evukK5l*GaHJ)a=%TCzgIM!xG8>Ls(q@aAf*nCwO5_}-Y`)@sCTtI{+to~PHK`j zkdi#J#WT|BaK8?!u(7&2;C-LIN1lNsCU}lTuX8ZMtZ7?0FZi=nJXgAzA_d_*u+!i4 zRAt1e6>t4IDx68k`;_KoZJOqCz)4au4 zWIdMM=?$WqE=~PhZsHl2bcie9N`pT4(lzBR&fAqk`EbcmSi;6OngcnZmX_XZoD6aYD1a&bQH%J7ahe{_0!)-(a|d;}j-awE zZKDEzj4VZ8kScJlXXr(!U-3%c4>UXuRK4DlFM`$-80;M-i0^qak9{iL#8olT(RsB) zhnDI=XLmVl%FbgM(9e-6Fh~zVwNW9sXwa=UtxB9jj7p6?%V;iD{7Gcw{k|ClV%%{7 zadcoM{67mo31^fRaEkp<7jreFr@I%nQO%D+TI<75OOXc0(BEAXjQR}f=)FS5DCL_; z4pV8(S-bFh;MHDB1)-w6emtn2Hv-fO-lhTT0$T|fclhGQUU#X4LC z3t)Sf-&SO~pTk-qnF-3^c(+@f2SQAkvD`e_&DBB{o>6NCPmC&8Hu98z=@DcCDixIgu@T}EEA8_YM?Yv9tRGisdZ9F=9tT&sikc5dmOb2U$ z!w0v_CwlP~QE{8Do91?hL(Z54oE4p<$hP0<^EOxh${Yj~AdSJJVGVeC-1yTnGc0N8 zn$hJcToKtX3KlPaE~oH(RyZ3QLH^dhxC(MXw4M7rLnRCojYtX1K_EhbR|O&tr_3JL zAWsI)plmG4rxTO;Q{bsYl3vt|p?Vw17~tj~ucyX+9mvl+baUo<3B?d03$0`PVC<~N zI93pDbA`$dsv}t+rjM=j<)X?lHx1+GO=b@ce?lehvSxM)f_JTyJ_oL)Yl^1DnPWL^ zCSmQnThOR)a6XC`QRczL=IoD`tO?GSn;g-z#dWK9()tYRF=d)X=;;x z0~Jk=R`8xuRjEtxxF8(l90bGv;!W(h+A(^^!?>xCHG0U9d+bJ zx^8L*$1Xr8aP%9z+FfrA7^g!z=4B(!h?qk#s&?EatCeYj$oouwBh~e& z9SPLtDk1CFEr6U>HWu?8RZ2mwGNLvZCKXr}n6BvygQTu|JYm8>sOhngmZ;a{zVl8m z>LpR^v90c@P8Uo;sPBpHw-AP03mH4-x5OogmjJ==t7e!4;!x`l?AfCa9S*E4LM!3o zHSU$xG;-W_o1T3j4h=;RShpgL@?gdBA|l9QcPLaO_Sx}7lXe)jKinNFzqUujva7b= zO73Zge7Q$!Ls|2#csP{)z=CzbrcI%{8GF3SH}4Bbze~=@^Vn#K|KvsZj_dq9aQB^y zI$@?esL>yO5Q=nqsf@*i#flPglRtzJqz6vJ6gJ}CwXx$eS4FAeoVyq+2DagcT}S&a zeoOeIm`cnXbRj?BJ>NguwryH`59|iQ{sC&Hgr|DaE^^cn^Eiq66 zShZRPge5U0>LNRiSieen+brd)U&zm|VCTVvU_tOkRXK_o1;Tg4MEG{Z0-x=>^P1!U z)U1ibtR|SW`_#xV-=UJyh9!-5ug$=T)B##tz<^eAL;}hnIxbsrO{j=h_r+5j=w`9? zMHfkND=ziM?8Jq7y5a~ju#&F?>SnY7%^1}bnFm3g&ztS#s@lhiPqzvfokVl1E$$CO z)7C#Co6|9t57WJXQrjLXD6x$jwcc*|-o1K~M*_8YVoNm1GUs!bpem8@!_5SX5rzV! z*4gM#RtCKDir@IXv0JqqE<{X7k#Xzr9W6QR(`6Go-b;obxI2&n?9H?f_>*lTz~lEZ zA#>Otw?(Z9D>d^=>^7tYB3ps19Z^*wPS}5`!pyRL5?MGSa}pX%kv||9X}43er=uJH zSgz?hR1tc@mWw>*&7b3h+Z0?{^gFyqA%e`*gOcbLC5wr@1_+7KS{by zt5hLjj0yt?R49sssgce3hBejq-0RZPP^9%KlDx10c}&YlGb)u`tF&Ih(D|p6KG%yJ zOkb~wqjHzUJZ}1u@>9>Xn+l0T%Ppq1<8Z)v$s4J%wngvF$3vmu=iHJy?Q|>#f9m~u zDfTlx)6_IY|CAgscCkXx*nKm!Qp;v#2DdM?aB=oVmD}9 zLYVJ*xw};}XLWhz)2&tLZ**JK+ZGS1i_h&HX-7BZSA|BrM7%m638RTiV8w>!q9FMu zrEbwCw4NayiaIa}yg{ma7L=h{h%BB|^ko!Cvur{e)^?v-AOrZAriGjoXv6ZqG7E>g zb@}nfs734E5K?(|J9VHFI>pTbb>=~?)>$|+rYXzia!7Vp#6Sl-^oiU^yN9kLun7@2 z_-TY-iq7jHAM2TqaZOfXD}APp*@g*MBc|<>f}6r%uB0_TOx8a8q^;pw(M%j8C%+vrc2TFi|8D<>fU*?Id2#$*j0G2L34 z0?!8TV=*BV6621Ii&WGDLs5)_pbkk8wI{TXP9NMe0$k8By*YthdSiGsHZJ~Pc7-`f zR56eS_&YZe1MYx7pKr_>SGWR37P<}^|LXJ{6|`6;VUk~`c!T%alT%GZy#~tqM(l-o zs=8xvXznYYIU8bjn}V+|;dDI1ZqL*W6y`3nGa6sNvlOM_I=secG~XxvQPOJ3cV$Qm zKo=qxy&vW1>zPL#NsUC;d6?ww+Kp7;}>-G_Zs`h_&a&ELU@@N$#|;Q70mi`sE48q-Rs?>*%E z4+$btjDaYVfONV8vJ5~KG%30!3D`3Fk}z&+e7xN7fvfv?^v$&CzQ7NbX1UTsLx0Vb})D&QCF1P^9I+xOun8O<~Bt`pyX4RGCnw>@%%O zZjRAc9&WFY%=a!F=IpaNXs~CX;}V3c%UlE=xc?M(QnPHBWt%e~(u!Q}y1C-_I&~I0 zrPf*^J!hqk@RxGqT|NvL-W|gL5tn^Ew9_w}y!puG7W!Wl#GK zWu+H1i6V_yvEH|bz7TOou0Tsg*0t)57gYd+lDsKH4j^MEkcV^Eyc>?WZZ~8F00vUT zw(MY$btVj7n7O3@kkf{kR77g~;!5yA^VxIELiSMoiRqJYXqx5}YpLKGrH}hc{RK<- z{W=56xtvo(z{S0?eaco9Wxa@~vAv7aTcQA%pb=dy^w>>6x%qefvV`P=H>RncM_a$# z%O6|Zt4p!$I@%$oMk}9Ggq*9`y9tSSpHQKJ9RUO zb$2N1Y91dsS{yEs8%fKv#J-ECw!SOTW}K|r@r zsL13d9xrHNcsSc!Esx5=X?wR*#8&ut`(`m#@%!DiVYTZS7TvvhsYw$YViusS8->si z?;V1;XUNTM`YFSDHC>q!?Y<2S+ZiU;-5y7H1WW}K1XP8NK9V8d+@)EmSgju3saSm5 z%xE;|_9x{2?J23k& z3t_7j=W@tv(v8q#Ryo>8-f~QK7@%6(mfH$wip1`@K)?`&YqAWWaXNKG9K;5`y`ud* zs}iMt6mLrmU|~_VqLTP&X$(AmFhS1J*f(yB_pW8gyU(a|aj`eQ%8L|Izz`PuaOERN z2-fOORCgBp=y$9=CV8$^5Zimqx;_l-h7LDXX2a$)gewIh89AFB{b-~J%MUCa_JRFXHpk{$CpxY3-+dxAs&u zwPl9q^;*P;K?qX`93wN;VJl{{HlOiMMOZ$IAh{HLleXUyxt7p<< zA?Rr9WAK{u)98BO>fuIKnC+OW2e?K=V-KcY=etQ=TO&z6l!8iD;0GXW9vQU@&sl1& zukyy!O_ggzM84P2Txk&u7p!o9UCsBbm~pbXbYd8BU8}|eD@2L1?r#GS^bw<;@XIdh z@y0w26RT>Gnu8F|gnsk-8E^>vctv)SpS9IWumjfrV;Rz&Ptso;`R25S9F9371EO9L z6R8E>GmgBwHy$2#xu>RC;3LPWG#n8kAG4=iSJKRnMGL*z4b|dn*dK^Ne5G{*SL?Qb zfu$aDzU?bCRNYpI-X%+g6fV8MV-0)OGL;BtayV8VPqpQmM<`0dFsG?vQZ0KlVySED z2{OO)*HP9*2&le1)(!a~<=?W|aUZk`qTkL*k-!+H>fv;l$mLd0?Ubx9xbLG%S4sM9 zihwD1gpWzZv=qJx`OPQ+u#H(Fma%XXu@3#ECg}S1`63aoF%r{xOVHp*xUt7o!2=U@ zDo51Pj@j56t#P)T0NXhB>8ekdF#f`f50$_8*S7|2K@q}`i2F_W3wI6NaTB=8+PPkA z-8awTExfrDjgk9=6c~qL%iHj?tGSD)3LQsCjnHsrkRVx4+g!YRk5YC_8>e7KCu{og zAEc4YXovSzJYrJ_;Z{k_NQ}4*daZn4G*cE=TTF?7gl-{Z%*%?JYh^;R-h>a~vL{1x zNiJt}A;L};fqW?==^ZSbaRIY|MDN(ylh6&7;aDf~xgW2gy!cGi5%?EDG(Kc>KXF#F zfYVSmh0B`;HD{*7mfauh;iNwo`m4eheQPKi91(6?hdREz9`q(SmLaEa955q0Wi_?5 z2-J@`k{DG=3wRX&67dSPWkXHqZx381+rc z&3=_!B)~Qvi@E;cjBSVXbtewR&df7b>{AE}NCZpUr-3qO*m=FiStW4~BpV}Z@W|vq z@O-3OFgcG1SIncqgaL0)gQFL`LY@5X(U%*i^Ev(2Jx@0V-I;7>a%I{nTwW%93$6YM z++lk?vAmRPwd$o&f}mg7hP5faf+dh3r^6ps6ht$e>SH}Gl{X(bhJ$|R=F zm7bxaca4n8(v%S0Jv57>C;Fis)qU|J=`(Jv`h{XAjv1>!_ygg&7=Z(+Ro5kKxT$P1 z4394dzR3STHJSsehcEqTv$oy;z1UVboyg!>fto`w4Dy&CX;dc={u3dM0-i%SqZ4CT z6ImTOj4)tuO|`lmneJ=jj;s;~AW)(DW;MNe?%JP{mu|c0G2qe9W9-CHN&ZLvXcf4z zHS;1s3y=ej;tyOlw-P@s>NJ0>hxJ2;>xrQm;_TNLO~-jmOZNmLU4$P)qhC3`J>Kl{ z7Q@Pc^{g?qObQ*HYOo(n#%3}F2G%D*)pL@;C$hs?Azu3k7xv<2Qij9|7@>0d9U=(X zhL2fzp8+2!-QlQA@KdpLP<{Nj23Vk2Y!cnP6e>MZXs5)Y(3ZAHimO(Q4s*vv6_eX( z;}t{vFc;DHv&8V4Lu7B45#Jl~=d;9vpJ!mdsy1cjmTmK;Rjm}%kLC8G+C%a}TL2I> zn4?TZ(OThL`rP~Kd9MMZv?)X^qQ@U|wXo+Y8pM!~_oTK~Av5)8btPX0WaE*boc=B{ zI0`%cudn#$^2!a($^t4wkop$4083l|D?*JAfDc?F=hf6D6xt%_%Qys=egexN%RPEW zTd6RnEE{6w&vpgR6ohGD7B*B&AbN0)P&yk&l0zm6aIV&`ho?phJ{CUCmyj>c(Rj87 z=8~vyJ*&@=i!FdE7#&X))v5SOYT%DIKN`>=K@zT)9eJ)!I$uHo%I|vNmZcOgfyESB zv=P>n40Aj`I!!3(dEJ{xl*KfW6Pb|Fl(*BUuP;jK2oi7zX`@2Q=hCuivoI!^inz6! zXA~6Iw-*E9*c}sC2*O`TEw6fn9|)4Q*XMdariJ4l%#>T+no8aGg^`pF;q7LhM_<7Z zryx&teCb*jR;x65B|nSd6u()6MbK5k6pTK3WTib1hkS*Hfk3>;Ff5L!{ep;1&v|q_ zop@$FnOjn0FQ_B;!*nY?vRQL>)6J*s5?W+}L?CuuE;jWlrd`_LNIBXGrPt>L4+nee z+5JUhOgONzKi?5tjis1Qrl*l^o)Yyv`V!;uF0igLLRVLr%o3@YAqVsFKseVI=2!0& z!$>xEjdY8+IeUIsFi$gGWkWu0*DN<6cQbB5=ZwPAy}zTmXQ_;yL&N8bSv;HiNrVyN z5K~b_M$#AtXLHb!ne2w>Wdsq>jMCB!fL>+`>f>ujtIZo0D$qy)rP@KjO%o?smpja> z$R|gu3ho0{4fhH)rMtTCjxiGfEs((EZdq-l>3!X zI;Yfh8u|=9wK}H?Lyn=;5zp}(tB|iB>0MF&Ir030uKc{7pB~{24w?vPSYh>dzT|Wz zJ8fYSZLE6?Fcw6{C(LTscl;297iQ%4(L*C?ItTZZnUQquy&HzYq;LQKhu%7amzG#g zbTFnQ0y@QW-!bltDN#Sl>K{M8umOn)Nefo+>-zmYvE(-|$HB zQAF>~&YocoqRnP;XpP~cEnITc?8a7`xO`tq@|ofkXy4ak8!|!-ER?{P#g@{mpA0FF zEi;F2PiY8AS*&;*>i*HFZWHFXS*jvkxQ>)McV)qkRy(UttM8}C7_djY3qs@v5W<|_S1(M z)q2_6FGsw~DBED}%J3`%>$&1M=hl~phld+GvZ@^s=uGZ*{OB(=7<88AhLsvTBDWL8 z>-^rlg8XhK3!WJ|Hg#4*KVN$@xY z)uy;gb?eZ z5=UqOF-HbkXD^^o3y$-$QGZ=rx6mzL9=TEdOAp-3Y*+4G_b7luP*|VOg1Yaa8Qj;;$X5>Y)yIo&}TR-=4FBXetK+vq;?H% z4d?+saST3VWZeVWR^(8Y8h_;7#m;nYbIYgh7_3I3uAns+;%5#djgGBwy#r!FiE z1QO zwuKl{`iZ|DGc-hyQ40RR#U6*k3nyS{zBcCP=lo|6QsLYPeDm9`2*1e_*IszLJ%BsqFMGb3wyP$`VMv>2E_5)9;PrOSs3i7^1SzGS=I$z64}x2%ooRN356|da)-U z*6KO4x`knxj!HFX=FQ!MT1b0EbiVh4e)ru}iACmyZps6Rr3WqYI}nNI;d-Ezi{_y? zdQ-mPK#nrYzdLfFr%E3nPf1iZ!)Mf>PfeiQtJd-X5+IC)h>JN4qNFmSlB6vXI z-?Z><7$uqNVPFaaHO(lHCd$Uq4oD9<1(rqG_HRNvFw$4HJ0Z77t^}Fb3#g&WO?_9| zLl2bF!g$aX0<(+Pd#%|p6%L>Sy>gdrPqVjc-6ThVi+IJEeYWd9X;GR{v(L8OLh#s~E^^sN(8#me}FoL+g3+C(L2?Wwo*hNuzN->Qi^j zsKYoFL9p++Tm-AW;#5pZe4?wx^SVQ7G1qI)r~4yk#IKT_&+0)`F_Rt&YYE`^*G1Q} z79_DZ1^MP@YR!*FE}zEQbOLVP!GBlPbceX{Yk&a(DG>cpRr6PDH?)sINlHkOk48*d znC_pf!H>f6@;E;0UV;x|z_-Bu28R4o0KZ*L(JV?nWcJ-ALAQ@?NbCxRhTV2a7^=h) z{nxhlM`D)NxY{`+eFi-2yWxd05KAwUDx1aox6(J=)>HExR4n+=Uv)*pT#^eldf#$y zs8UGezKO=FAA;)cubcphy`3iIP1mP>b~VAMyR}Vis!l=?eU8Y;o%TZVQ}8hjA~q^m zSgs>536L{uEwPrF9N^bl-gz|UHL*C(KA8M!x3EA1b`Vi=h?A{H1!l2470sqUI0*Of za>yA7RU!ciq^}>EAQQEG^X05%JYa=mGxyP9(<94KX z@0UA_-%;<79O-BLDAND(5qVh&U=UQGzvuk^o;LoJKN!IOsrBa>KtR9Aq<@OyNAtgN zeE+@oKlT2nmg>*_h`+%3lk@xcD8DJHe@f3s^M6_MZyeyiNBUjc^{2diH2)LnPfqaP zqx^ex7(TATf1>;=NBHk?ezS!C6!X8p`CpvjzX$tw*M9Ri{?n*GImCaD^6###{RPUu za*F>R=ie_G8p!{)=HEHS|AX|$!11pH+TYpCpCa>t^v}`bgLC}HP5g)EzY>#vSLyvJ zKR!zI{-oRfr{2GEUVhV-{uJ8}m=DVQKS)gfsr9b~g}?bQe~Q$H*#>{IWByS5yL!Vv z70EFEzWVSFls}yP*OB+%!?e%*?{ER*Z Kohy#td;bTD$oyph literal 0 HcmV?d00001 diff --git a/motorControl.ino b/motorControl.ino index 6b24aca..736d6f3 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -169,7 +169,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl windup = saturation - speedCMD_; speedCMD_ = saturation; } else if (speedCMD_ < -saturation) { - windup = saturation - speedCMD_; + windup = -saturation - speedCMD_; speedCMD_ = -saturation; } From 6e266b801a0614b737d6e5543e88a15ac78f8fd2 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 12:05:59 +0200 Subject: [PATCH 21/22] Updated dependencies in Readme --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index a5261a7..bb0a32a 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,11 @@ These settings allow upload: ## Dependencies ### Boards Manager [esp32 v1.0.4](https://github.com/espressif/arduino-esp32) -#### Libraries +#### Boards libraries - Wire @1.0.1 +- Wifi @1.0 +- ESP32 Async UDP @1.0.0 + ### Libraries [Ps3Controller.h @v1.1.0](https://github.com/jvpernis/esp32-ps3) From 212597e5e694808920646f9e490298eef4625d44 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sun, 22 Oct 2023 12:06:14 +0200 Subject: [PATCH 22/22] Cleanup --- Balancebot.ino | 13 +++++-------- UDP.ino | 3 --- interruptEncoders.ino | 3 +-- motorControl.ino | 14 ++++++-------- 4 files changed, 12 insertions(+), 21 deletions(-) diff --git a/Balancebot.ino b/Balancebot.ino index fae01b6..1f08c1b 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -2,7 +2,6 @@ #include #include #include -#include #include //Declare library objects @@ -40,13 +39,17 @@ long int m2Raw, m2RawLast; volatile bool M1_A_state, M1_B_state; volatile bool M2_A_state, M2_B_state; + //PS3 Controller variables const char* _ps3Address = "18:5e:0f:92:00:6c"; + //UDP variables uint8_t data[30 * 4]; + void setup() { + //Initialize serial Serial.begin(9600); delay(10); @@ -55,12 +58,8 @@ void setup() { Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); delay(10); - //Initialize IMU - Serial.println("Before IMU init"); IMU.init(); - Serial.println("After IMU init"); - delay(10); //Initialize encoder interrupts @@ -94,17 +93,15 @@ void setup() { } void loop() { - // Serial.println("Loop"); + //Update time variables tNow = micros(); dT = tNow - tLast; //[Cycle time in microseconds] dT_s = dT * pow(10, -6); //[Cycle time in seconds] - //Get sensor data readIMU(); - //Control motors motors(); diff --git a/UDP.ino b/UDP.ino index 19e6b33..39141e8 100644 --- a/UDP.ino +++ b/UDP.ino @@ -11,16 +11,13 @@ byte watchdog = 0; AsyncUDP udp; void UdpInit() { - //Serial.begin(115200); ConnectToWiFi(); - //udp.connect(multicastIP, port); } void UdpLoop() { udp.writeTo(data, sizeof(data), multicastIP, port); } - void ConnectToWiFi() { WiFi.mode(WIFI_STA); WiFi.begin(ssid, password); diff --git a/interruptEncoders.ino b/interruptEncoders.ino index 12aa1a7..b9a02d6 100644 --- a/interruptEncoders.ino +++ b/interruptEncoders.ino @@ -93,7 +93,6 @@ void IRAM_ATTR m2_B_changed() { } } - void initEncoderInterrupt() { pinMode(M1_ENC_A, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); @@ -106,4 +105,4 @@ void initEncoderInterrupt() { pinMode(M2_ENC_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); -} +} \ No newline at end of file diff --git a/motorControl.ino b/motorControl.ino index 736d6f3..1d61c0f 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -65,16 +65,16 @@ void motors() { IMU.init(); } + //Calculate wheel angular velocity + motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); - //Calculate wheel angular velocity - motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); - motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); //Calculate robot linear and angular velocity Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); //Get Control Commands - rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.lx, -128.0, 127.0, -3.75, 3.75); if (balancingOn) { @@ -157,6 +157,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl byte ch2 = motorID * 2; byte ch1 = ch2 - 1; float windup = 0; + //Deadband if (speedCMD_ > 0 && speedCMD_ < dbPos_) { speedCMD_ = dbPos_; @@ -171,13 +172,10 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl } else if (speedCMD_ < -saturation) { windup = -saturation - speedCMD_; speedCMD_ = -saturation; - } - - else { + } else { speedCMD_ = speedCMD_; } - //Apply speed command to PWM output if (speedCMD_ > 0) { ledcWrite(ch1, 0);