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

Balance controller is not working

This commit is contained in:
Stedd 2023-10-21 15:55:08 +02:00
parent c58b413392
commit 62e07ce32a
4 changed files with 104 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Balancebot.ino
View File

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

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

36
motorControl.ino
View File

@ -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,6 +48,20 @@ void initMotors() {
void motors() {
if (Ps3.data.button.cross) {
ResetIntegrators();
balancingOn = true;
}
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);
@ -76,13 +91,18 @@ void motors() {
//Turn controller
TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[1][0], 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;
//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;
@ -91,17 +111,19 @@ void motors() {
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_;
}

55
plot.ino
View File

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