Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Merge branch 'SketchStructure'

This commit is contained in:
Stedd 2023-10-22 14:30:30 +02:00
parent a0ec9092b0 0aa6e9c5ad
commit eb9700a84f
9 changed files with 141 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
arduino_secrets.h

70
Balancebot.ino
View File

@ -3,6 +3,7 @@
#include <Wire.h>
#include <MatrixMath.h>
#include <Ps3Controller.h>
#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();

17
IMU.ino
View File

@ -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);

14
README.md
View File

@ -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

64
UDP.ino
View File

@ -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;
}

0
Spreadsheets/MotorSaturation_Bug.ods → data/MotorSaturation_Bug.ods
View File

40
motorControl.ino
View File

@ -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;

79
plot.ino
View File

@ -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;
// }

13
sketch.yaml Normal file
View File

@ -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)