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

Balance Controller kind of works 

Not sure why the gains dont work anymore.
Anyways, continuing with UDP publishing.
This commit is contained in:
Stedd 2023-10-21 17:51:45 +02:00
parent 62e07ce32a
commit b8e6e120d4
3 changed files with 37 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Balancebot.ino
View File

@ -45,7 +45,7 @@ const char* _ps3Address = "18:5e:0f:92:00:6c";
void setup() { void setup() {
//Initialize serial //Initialize serial
Serial.begin(19200); Serial.begin(9600);
delay(10); delay(10);
//Initialice I2C //Initialice I2C

20
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,7 +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 = false; bool balancingOn = true;
//Matrices //Matrices
mtx_type motor_ang_vel[2][1]; mtx_type motor_ang_vel[2][1];
@ -61,6 +61,10 @@ void motors() {
ResetIntegrators(); ResetIntegrators();
} }
if (Ps3.data.button.square) {
IMU.init();
}
if (balancingOn) { if (balancingOn) {
//Calculate wheel angular velocity //Calculate wheel angular velocity
@ -92,6 +96,7 @@ void motors() {
//Turn controller //Turn controller
TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC); TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC);
//Sum speed command for motors //Sum speed command for motors
M1_Speed_CMD = IL_cont_out - TC_cont_out; M1_Speed_CMD = IL_cont_out - TC_cont_out;
M2_Speed_CMD = IL_cont_out + TC_cont_out; M2_Speed_CMD = IL_cont_out + TC_cont_out;
@ -173,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);

45
plot.ino
View File

@ -8,18 +8,17 @@ void plot() {
// Serial.print(" "); // Serial.print(" ");
// IMU // IMU
Serial.print("Pitch:");
Serial.println(pitch); // Serial.print("RollRate:");
// Serial.println(pitch_rate);
// Serial.print("Accelerometer_Pitch:"); // Serial.print("Accelerometer_Pitch:");
// Serial.println(acc_pitch); // Serial.println(acc_pitch);
Serial.print("RollRate"); // Serial.print("Pitch:");
Serial.println(gz); // Serial.println(pitch);
// Serial.print ( "," ); // Serial.print ( "," );
// Serial.println ( gt ); // Serial.println ( gt );
// Serial.print ( " " ); // Serial.print ( " " );
// Serial.println ( acc_pitch); // Serial.println ( acc_pitch);
@ -39,41 +38,35 @@ 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.println(SC_cont_out); // Serial.println(SC_cont_out);
Serial.print("BalanceOLControllerOut:"); // Serial.print("BalanceOLControllerOut:");
Serial.println(OL_cont_out); // Serial.println(OL_cont_out);
Serial.print("BalanceILControllerOut:"); // Serial.print("BalanceILControllerOut:");
Serial.println(IL_cont_out); // Serial.println(IL_cont_out);
Serial.print("AntiWindup:"); // Serial.print("TurnControllerOut:");
Serial.println(IL_anti_windup); // Serial.println(TC_cont_out);
// Serial.print("SpeedCmd1:");
// Serial.println(speedCmd1);
// Serial.print(" ");
// Serial.print("M1_CMD:"); // Serial.print("M1_CMD:");
// Serial.println(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);
// Serial.print("M1_Ang_Vel:"); // Serial.print("M1_Ang_Vel:");
// Serial.println(motor_ang_vel[0][0]); // Serial.print(motor_ang_vel[0][0]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("M2_Ang_Vel:"); // Serial.print("M2_Ang_Vel:");
// Serial.println(motor_ang_vel[0][1]); // Serial.print(motor_ang_vel[0][1]);
// Serial.print(" "); // Serial.print(" ");
// Serial.print("botLinVel:"); // Serial.print("botLinVel:");
// Serial.println(vel_Matrix[0][0]); // Serial.println(vel_Matrix[0][0]);