diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5a50517 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +arduino_secrets.h diff --git a/Balancebot.ino b/Balancebot.ino index 1f08c1b..6e1960b 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -3,6 +3,7 @@ #include #include #include +#include "arduino_secrets.h" //Declare library objects GY_85 IMU; @@ -28,12 +29,7 @@ int dT = 0; float dT_s = 0.0; -//Motor variables -const int PWM_CYCLE = 12000; -const byte PWM_RES = 12; - - -//Encoders variables +//Encoder variables long int m1Raw, m1RawLast; long int m2Raw, m2RawLast; volatile bool M1_A_state, M1_B_state; @@ -44,6 +40,66 @@ volatile bool M2_A_state, M2_B_state; const char* _ps3Address = "18:5e:0f:92:00:6c"; +//MotorControl Constants +const int PWM_CYCLE = 12000; +const byte PWM_RES = 12; +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; + + +//MotorControl 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 + + +//MotorControl 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; + + +//MotorControl Matrices +mtx_type motor_ang_vel[2][1]; +mtx_type vel_Matrix[2][1]; +mtx_type inv_Kin[2][2]; + + +//IMU 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; + + //UDP variables uint8_t data[30 * 4]; @@ -106,7 +162,7 @@ void loop() { motors(); // Plot - plot(); + SerialPlot(); //Udp UdpLoop(); diff --git a/IMU.ino b/IMU.ino index 83ff38b..55c8455 100644 --- a/IMU.ino +++ b/IMU.ino @@ -1,20 +1,3 @@ -//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); diff --git a/README.md b/README.md index bb0a32a..6580ce1 100644 --- a/README.md +++ b/README.md @@ -8,16 +8,4 @@ These settings allow upload: ## 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) +See sketch.yaml diff --git a/UDP.ino b/UDP.ino index 39141e8..1cd44f4 100644 --- a/UDP.ino +++ b/UDP.ino @@ -1,5 +1,5 @@ -const char* ssid = "CaveBot"; -const char* password = "&nHM%D2!$]Qg[VUv"; +const char* ssid = SECRET_SSID; +const char* password = SECRET_PASSWORD; #include "WiFi.h" #include "AsyncUDP.h" @@ -15,6 +15,7 @@ void UdpInit() { } void UdpLoop() { + PackUdpData(); udp.writeTo(data, sizeof(data), multicastIP, port); } @@ -28,3 +29,62 @@ void ConnectToWiFi() { } } } + +void PackUdpData() { + 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; +} diff --git a/Spreadsheets/MotorSaturation_Bug.ods b/data/MotorSaturation_Bug.ods similarity index 100% rename from Spreadsheets/MotorSaturation_Bug.ods rename to data/MotorSaturation_Bug.ods diff --git a/motorControl.ino b/motorControl.ino index 1d61c0f..02970fa 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -1,43 +1,3 @@ -//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; diff --git a/plot.ino b/plot.ino index a1d051f..c0e6087 100644 --- a/plot.ino +++ b/plot.ino @@ -1,4 +1,4 @@ -void plot() { +void SerialPlot() { // Time // Serial.print("dT:"); // Serial.println(dT); @@ -8,13 +8,10 @@ void plot() { // 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 ( "," ); @@ -39,29 +36,21 @@ void plot() { // 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(" "); @@ -88,70 +77,4 @@ 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, 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; -// } diff --git a/sketch.yaml b/sketch.yaml new file mode 100644 index 0000000..c137d83 --- /dev/null +++ b/sketch.yaml @@ -0,0 +1,13 @@ +profiles: + Esp32BalanceBot: + fqbn: arduino:esp32:esp32:firebeetle32 + platforms: + - platform: arduino:esp32 (1.0.4) + platform_index_url: https://dl.espressif.com/dl/package_esp32_index.json + libraries: + - GY85 (1.0) + - Wire (1.0.1) + - MatrixMath (1.0) + - PS3 Controller Host (1.1.0) + - WiFi (1.0) + - ESP32 Async UDP (1.0.0) \ No newline at end of file