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

Compare commits

base: Stedd:c58b413392e9aec41eb0d48976a20ec8408b89aa
Branches Tags
Stedd:master
...
compare: Stedd:b8e6e120d46a3510355634f8d6710a2d9eb65d7b
Branches Tags
Stedd:master

2 Commits
c58b413392 ... b8e6e120d4

Author SHA1 Message Date
Stedd b8e6e120d4 Balance Controller kind of works 
Not sure why the gains dont work anymore.
Anyways, continuing with UDP publishing.
 2023-10-21 17:51:45 +02:00
Stedd 62e07ce32a Balance controller is not working 2023-10-21 15:55:08 +02:00
4 changed files with 110 additions and 87 deletions
Show Stats 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() { void setup() {
//Initialize serial //Initialize serial
Serial.begin(57600); Serial.begin(9600);
delay(10); delay(10);
//Initialice I2C //Initialice I2C
Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
//delay(10); delay(10);
//Initialize IMU //Initialize IMU
Serial.println("Before IMU init"); Serial.println("Before IMU init");
IMU.init(); IMU.init();
//IMU.init();
Serial.println("After IMU init"); Serial.println("After IMU init");
delay(10); delay(10);
//Initialize encoder interrupts //Initialize encoder interrupts

30
IMU.ino
View File

@ -7,7 +7,7 @@ const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
//IMU VARIABLES //IMU VARIABLES
int ax, ay, az; int ax, ay, az;
int cx, cy, cz; int cx, cy, cz;
float gx, gy, gz; int gx, gy, gz;
float gt; float gt;
float acc_pitch; float acc_pitch;
float pitch_rate; float pitch_rate;
@ -16,30 +16,26 @@ float pitch_prev = 0;
void readIMU() { void readIMU() {
// Serial.println("ReadingIMU");
//Acceletometer //Acceletometer
int* accelerometerReadings = IMU.readFromAccelerometer(); ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);
ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value); ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value);
ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value); az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value);
az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value);
//Magnetometer //Magnetometer
int* compassReadings = IMU.readFromCompass(); cx = IMU.compass_x(IMU.readFromCompass());
cx = IMU.compass_x(compassReadings); cy = IMU.compass_y(IMU.readFromCompass());
cy = IMU.compass_y(compassReadings); cz = IMU.compass_z(IMU.readFromCompass());
cz = IMU.compass_z(compassReadings);
// // Gyrocope // Gyrocope
// float* gyroReadings = IMU.readGyro(); gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate
// gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate
// gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate
// gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate
// //Temperature sensor //Temperature sensor
// gt = IMU.temp(gyroReadings); gt = IMU.temp(IMU.readGyro());
// Pitch angle from accelerometer // Pitch angle from accelerometer

110
motorControl.ino
View File

@ -13,14 +13,14 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float K_SC = 18.5; //Speed controller gain const float gainScale = 0.75;
const float K_TC = 90.0; //Turn controller gain const float K_SC = 18.5*gainScale; //Speed controller gain
const float K_OL = 13.0; //Outer loop balance controller gain const float K_TC = 90.0*gainScale; //Turn controller gain
const float K_IL = 72.0; //Inner loop balance controller gain const float K_OL = 13.0*gainScale; //Outer loop balance controller gain
const float I_IL = 80.0; //Inner loop balance controller Igain 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 const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables //Help variables
int M1_Speed_CMD, M2_Speed_CMD; int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref; float rem_speed_ref, rem_turn_speed_ref;
@ -30,6 +30,7 @@ float OL_cont_out;
float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
float speedCmd1, speedCmd2; float speedCmd1, speedCmd2;
bool balancingOn = true;
//Matrices //Matrices
mtx_type motor_ang_vel[2][1]; mtx_type motor_ang_vel[2][1];
@ -47,61 +48,87 @@ void initMotors() {
void motors() { void motors() {
if (Ps3.data.button.cross) {
ResetIntegrators();
balancingOn = true;
}
//Calculate wheel angular velocity if (Ps3.data.button.circle) {
motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); balancingOn = false;
motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); }
if (Ps3.data.button.triangle) {
ResetIntegrators();
}
if (Ps3.data.button.square) {
IMU.init();
}
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);
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 //Calculate robot linear and angular velocity
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
//Get Control Commands //Get Control Commands
rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75);
rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35);
// Speed Controller // Speed Controller
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
// Balance controller // Balance controller
// Outer loop // Outer loop
OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
// Inner loop // Inner loop
ref_IL = OL_cont_out; ref_IL = OL_cont_out;
act_IL = pitch_rate; act_IL = pitch_rate;
error_IL = ref_IL - act_IL; 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)))); 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); IL_cont_out = round((error_IL * K_IL) + iError_IL);
//Turn controller //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;
//Sum speed command for motors //Motor control
M1_Speed_CMD = IL_cont_out - TC_cont_out; IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG);
M2_Speed_CMD = IL_cont_out + TC_cont_out; 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;
//Sum speed command for motors } else {
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); //Sum speed command for motors
M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0);
motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); 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;
//Update variables for next scan cycle //Update variables for next scan cycle
m1RawLast = m1Raw; m1RawLast = m1Raw;
m2RawLast = m2Raw; m2RawLast = m2Raw;
} }
void ResetIntegrators() {
iError_IL = 0.0;
IL_anti_windup = 0.0;
}
float PController(float ref_, float act_, float k_) { float PController(float ref_, float act_, float k_) {
return (ref_ - act_) * k_; return (ref_ - act_) * k_;
} }
@ -151,6 +178,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
speedCMD_ = speedCMD_; speedCMD_ = speedCMD_;
} }
//Apply speed command to PWM output //Apply speed command to PWM output
if (speedCMD_ > 0) { if (speedCMD_ > 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);

50
plot.ino
View File

@ -8,14 +8,15 @@ void plot() {
// Serial.print(" "); // Serial.print(" ");
// IMU // IMU
Serial.print ( "Pitch:" );
Serial.println ( pitch ); // Serial.print("RollRate:");
// Serial.print (" "); // Serial.println(pitch_rate);
// Serial.print ( "Accelerometer_Pitch:" );
// Serial.print ( acc_pitch ); // Serial.print("Accelerometer_Pitch:");
// Serial.print (" "); // Serial.println(acc_pitch);
// Serial.print ( "," );
// Serial.println ( gz ); // Serial.print("Pitch:");
// Serial.println(pitch);
// Serial.print ( "," ); // Serial.print ( "," );
// Serial.println ( gt ); // Serial.println ( gt );
// Serial.print ( " " ); // Serial.print ( " " );
@ -37,30 +38,27 @@ void plot() {
// Encoders // Encoders
// Serial.print("m1Raw:"); // Serial.print("m1Raw:");
// Serial.print(m1Raw); // Serial.println(m1Raw);
// Serial.print(" ");
// Serial.print("m2Raw:"); // Serial.print("m2Raw:");
// Serial.println(m2Raw); // Serial.println(m2Raw);
// Motors // // Motors
// Serial.print("SpeedControllerOut:"); // Serial.print("SpeedControllerOut:");
// Serial.print(SC_cont_out); // Serial.println(SC_cont_out);
// Serial.print(" ");
// Serial.print("BalanceOLControllerOut:"); // Serial.print("BalanceOLControllerOut:");
// Serial.print(OL_cont_out); // Serial.println(OL_cont_out);
// Serial.print(" ");
// Serial.print("BalanceILControllerOut:"); // Serial.print("BalanceILControllerOut:");
// Serial.print(IL_cont_out); // Serial.println(IL_cont_out);
// Serial.print(" ");
// Serial.print("SpeedCmd1:"); // Serial.print("TurnControllerOut:");
// Serial.println(speedCmd1); // Serial.println(TC_cont_out);
// Serial.print(" ");
// Serial.print("M1_CMD:"); // Serial.print("M1_CMD:");
// Serial.print(M1_Speed_CMD); // Serial.println(M1_Speed_CMD);
// Serial.print(" ");
// Serial.print("SpeedCmd2:");
// Serial.println(speedCmd2);
// Serial.print(" ");
// Serial.print("M2_CMD:"); // Serial.print("M2_CMD:");
// Serial.println(M2_Speed_CMD); // Serial.println(M2_Speed_CMD);
@ -71,7 +69,7 @@ void plot() {
// Serial.print(motor_ang_vel[0][1]); // Serial.print(motor_ang_vel[0][1]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("botLinVel:"); // Serial.print("botLinVel:");
// Serial.print(vel_Matrix[0][0]); // Serial.println(vel_Matrix[0][0]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("botAngVel:"); // Serial.print("botAngVel:");
// Serial.println(vel_Matrix[1][0]); // Serial.println(vel_Matrix[1][0]);