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

quicksave

This commit is contained in:
Stedd 2023-10-19 00:26:26 +02:00
parent b5a53fa8c9
commit 72716f2abd
5 changed files with 55 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
Main/IMU.ino
View File

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

14
Main/Main.ino
View File

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

16
Main/motorControl.ino
View File

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

9
Main/plot.ino
View File

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

16
README.md
View File

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