diff --git a/Balancebot.ino b/Balancebot.ino new file mode 100644 index 0000000..1f08c1b --- /dev/null +++ b/Balancebot.ino @@ -0,0 +1,119 @@ +//Import +#include +#include +#include +#include + +//Declare library objects +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 int IMU_I2C_SDA = 26; +const int IMU_I2C_SCL = 27; + + +//Time variables +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; + + +//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; + + +//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); + + //Initialice I2C + Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); + delay(10); + + //Initialize IMU + IMU.init(); + delay(10); + + //Initialize encoder interrupts + initEncoderInterrupt(); + + //Initialize encoders + m1Raw = 0; + m1RawLast = 100; + m2Raw = 0; + m2RawLast = 100; + + // Initialize PWM channels + ledcAttachPin(M1_A, 1); + ledcAttachPin(M1_B, 2); + ledcAttachPin(M2_A, 3); + ledcAttachPin(M2_B, 4); + + 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 PS3 controller connection + Ps3.begin(_ps3Address); + + //Init UDP + UdpInit(); +} + +void 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(); + + // Plot + plot(); + + //Udp + UdpLoop(); + + //Save time for next cycle + tLast = tNow; + + //Delay + delay(5); +} diff --git a/IMU.ino b/IMU.ino new file mode 100644 index 0000000..83ff38b --- /dev/null +++ b/IMU.ino @@ -0,0 +1,65 @@ +//CONSTANTS +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; + + +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); + + + //Magnetometer + 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 + + + //Temperature sensor + gt = IMU.temp(IMU.readGyro()); + + + // Pitch angle from accelerometer + acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI; + + + //Pitch rate from gyroscope + pitch_rate = -gx; + + + //Complementary filter + pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); + pitch_prev = pitch; +} + + +int convertInt(int raw, int overflow_value_) { + //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. + + if (raw > (overflow_value_ / 2)) { + return (raw - overflow_value_); + } + + else { + return raw; + } +} diff --git a/Main/IMU.ino b/Main/IMU.ino deleted file mode 100644 index b42cf88..0000000 --- a/Main/IMU.ino +++ /dev/null @@ -1,67 +0,0 @@ -//CONSTANTS -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; - - -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); - - - //Magnetometer - 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 - - - //Temperature sensor - gt = IMU.temp ( IMU.readGyro() ); - - - // Pitch angle from accelerometer - acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI; - - - //Pitch rate from gyroscope - pitch_rate = -gx; - - - //Complementary filter - pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); - pitch_prev = pitch; - - -} - - -int convertInt(int raw, int overflow_value_) { - //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK. - - if (raw > (overflow_value_ / 2)) { - return (raw - overflow_value_); - } - - else { - return raw; - } -} diff --git a/Main/Main.ino b/Main/Main.ino deleted file mode 100644 index b4dd646..0000000 --- a/Main/Main.ino +++ /dev/null @@ -1,111 +0,0 @@ -//Import -#include -#include -#include - - -//Declare library objects -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; - - -//Time variables -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; - - -//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; - - -void setup() { - //Initialize serial - Serial.begin(57600); - delay(10); - - //Initialice I2C - Wire.begin(IMU_I2C_SCL, IMU_I2C_SDA); - delay(10); - - //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(); - - //Initialize encoders - m1Raw = 0; - m1RawLast = 100; - m2Raw = 0; - m2RawLast = 100; - - // Initialize PWM channels - ledcAttachPin(M1_A, 1); - ledcAttachPin(M1_B, 2); - ledcAttachPin(M2_A, 3); - ledcAttachPin(M2_B, 4); - - 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(); - -} - -void 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(); - - - // Plot - plot(); - - - //Save time for next cycle - tLast = tNow; - - - //Delay - delay(5); -} 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 deleted file mode 100644 index e753074..0000000 --- a/Main/motorControl.ino +++ /dev/null @@ -1,178 +0,0 @@ -//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; - - -//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 - - -//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; - - - -//Matrices -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; -} - -void motors() { - - - //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); - - - // 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); - } - - - // 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); - - - //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; - - //Sum speed command for motors - // 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; - - //Update variables for next scan cycle - m1RawLast = m1Raw; - m2RawLast = m2Raw; - - -} - -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] - 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 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; - //Deadband - if (speedCMD_ > 0 && speedCMD_ < dbPos_) { - speedCMD_ = dbPos_; - } - else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { - speedCMD_ = -dbNeg_; - } - - // Speed command saturation - else if (speedCMD_ > saturation) { - windup = saturation-speedCMD_; - speedCMD_ = saturation; - } - else if (speedCMD_ < -saturation) { - windup = saturation-speedCMD_; - speedCMD_ = -saturation; - } - - else { - speedCMD_ = speedCMD_; - } - - //Apply speed command to PWM output - if (speedCMD_ > 0) { - ledcWrite(ch1, 0); - ledcWrite(ch2, speedCMD_); - } - else if (speedCMD_ < 0) { - ledcWrite(ch1, -1 * speedCMD_); - ledcWrite(ch2, 0); - } - else if (speedCMD_ == 0) { - ledcWrite(ch1, 0); - ledcWrite(ch2, 0); - } - - return windup; - -} diff --git a/Main/plot.ino b/Main/plot.ino deleted file mode 100644 index c0ea6b0..0000000 --- a/Main/plot.ino +++ /dev/null @@ -1,75 +0,0 @@ -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); - - - // 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); - - - // Encoders - // 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); - - // 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]); -} diff --git a/README.md b/README.md index f23c65d..bb0a32a 100644 --- a/README.md +++ b/README.md @@ -1 +1,23 @@ -# Balancebot +# BalanceBot + +## Arduino Board Settings +My board has this etched on it: ESP-WROOM-32 + +These settings allow upload: +* Board: FireBeetle-ESP32 + + +## Dependencies +### Boards Manager +[esp32 v1.0.4](https://github.com/espressif/arduino-esp32) +#### 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) + +[MatrixMath.h @v1.0](https://github.com/eecharlie/MatrixMath) + +[GY_85.h @commit af33c9f791618cec6ae218fe73d039276448ff4f](https://github.com/sqrtmo/GY-85-arduino/tree/master) diff --git a/Spreadsheets/MotorSaturation_Bug.ods b/Spreadsheets/MotorSaturation_Bug.ods new file mode 100644 index 0000000..e256e3a Binary files /dev/null and b/Spreadsheets/MotorSaturation_Bug.ods differ diff --git a/UDP.ino b/UDP.ino new file mode 100644 index 0000000..39141e8 --- /dev/null +++ b/UDP.ino @@ -0,0 +1,30 @@ +const char* ssid = "CaveBot"; +const char* password = "&nHM%D2!$]Qg[VUv"; + +#include "WiFi.h" +#include "AsyncUDP.h" + +const IPAddress multicastIP = IPAddress(239, 1, 2, 3); +int port = 1234; + +byte watchdog = 0; +AsyncUDP udp; + +void UdpInit() { + ConnectToWiFi(); +} + +void UdpLoop() { + 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); + } + } +} diff --git a/Main/interruptEncoders.ino b/interruptEncoders.ino similarity index 85% rename from Main/interruptEncoders.ino rename to interruptEncoders.ino index 29ca3a7..b9a02d6 100644 --- a/Main/interruptEncoders.ino +++ b/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,22 @@ 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); -} +} \ No newline at end of file diff --git a/motorControl.ino b/motorControl.ino new file mode 100644 index 0000000..1d61c0f --- /dev/null +++ b/motorControl.ino @@ -0,0 +1,192 @@ +//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; + + +//Tuning +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; +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; +float speedCmd1, speedCmd2; + +bool balancingOn = false; + +//Matrices +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; +} + +void motors() { + + if (Ps3.data.button.cross) { + ResetIntegrators(); + balancingOn = true; + } + + if (Ps3.data.button.circle) { + balancingOn = false; + } + + if (Ps3.data.button.triangle) { + ResetIntegrators(); + } + + if (Ps3.data.button.square) { + 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 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_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) { + + + // 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); + + + //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; + + } 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); + + 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); + } + + + //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_; +} + + +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 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; + + //Deadband + if (speedCMD_ > 0 && speedCMD_ < dbPos_) { + speedCMD_ = dbPos_; + } else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { + speedCMD_ = -dbNeg_; + } + + // Speed command saturation + else if (speedCMD_ > saturation) { + windup = saturation - speedCMD_; + speedCMD_ = saturation; + } else if (speedCMD_ < -saturation) { + windup = -saturation - speedCMD_; + speedCMD_ = -saturation; + } else { + speedCMD_ = speedCMD_; + } + + //Apply speed command to PWM output + if (speedCMD_ > 0) { + ledcWrite(ch1, 0); + ledcWrite(ch2, speedCMD_); + } else if (speedCMD_ < 0) { + ledcWrite(ch1, -1 * speedCMD_); + ledcWrite(ch2, 0); + } else if (speedCMD_ == 0) { + ledcWrite(ch1, 0); + ledcWrite(ch2, 0); + } + + return windup; +} diff --git a/plot.ino b/plot.ino new file mode 100644 index 0000000..a1d051f --- /dev/null +++ b/plot.ino @@ -0,0 +1,157 @@ +void plot() { + // Time + // Serial.print("dT:"); + // Serial.println(dT); + // Serial.print(" "); + // Serial.print("dT_s:"); + // Serial.println(dT_s); + // Serial.print(" "); + + // IMU + + // Serial.print("RollRate:"); + // Serial.println(pitch_rate); + + // Serial.print("Accelerometer_Pitch:"); + // Serial.println(acc_pitch); + + // Serial.print("Pitch:"); + // Serial.println(pitch); + // 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); + + + // Encoders + // Serial.print("m1Raw:"); + // Serial.println(m1Raw); + + // Serial.print("m2Raw:"); + // Serial.println(m2Raw); + + // // Motors + // 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("TurnControllerOut:"); + // Serial.println(TC_cont_out); + + // Serial.print("M1_CMD:"); + // Serial.println(M1_Speed_CMD); + + // 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.println(vel_Matrix[0][0]); + // 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); + // } + + 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, 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) { + 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]; +}; + +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; +// }