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

Compare commits

base: Stedd:6a03848435ac16248a6f124fbc1e39a0ce6b7328
Branches Tags
Stedd:master
...
compare: Stedd:26955c1c123894b9cb03a061a6278128b25e03bc
Branches Tags
Stedd:master

5 Commits
6a03848435 ... 26955c1c12

Author SHA1 Message Date
Stedd 26955c1c12 Formatting 2023-10-22 02:09:46 +02:00
Stedd 1fa5f97b25 Moved velocity calculations out of balancing loop so they always can be monitored 2023-10-22 02:09:38 +02:00
Stedd 3343c2ac98 Added some more variables to UDP 2023-10-22 02:08:19 +02:00
Stedd 141a03cb84 Bot can balance by itself again 2023-10-22 02:07:22 +02:00
Stedd 3a680f84c8 Fixed a bug in LeftMotorAngular velocity calculation 2023-10-22 02:06:40 +02:00
2 changed files with 48 additions and 32 deletions
Show Stats Expand all files Collapse all files

39
motorControl.ino
View File

@ -13,13 +13,13 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float gainScale = 0.75; const float gainScale = 1;
const float K_SC = 18.5*gainScale; //Speed controller gain const float K_SC = 18.5 * gainScale; //Speed controller gain
const float K_TC = 90.0*gainScale; //Turn 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_OL = 13.0 * gainScale; //Outer loop balance controller gain
const float K_IL = 72.0*gainScale; //Inner 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 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;
@ -62,27 +62,26 @@ void motors() {
} }
if (Ps3.data.button.square) { if (Ps3.data.button.square) {
IMU.init(); IMU.init();
} }
//Calculate wheel angular velocity
motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[0][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
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
//Get Control Commands
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);
if (balancingOn) { 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
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
//Get Control Commands
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);
// 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);

41
plot.ino
View File

@ -91,8 +91,8 @@ void plot() {
int i = 0; int i = 0;
data[i] = watchdog++; data[i] = watchdog++;
data[i += 1] = balancingOn<<1; data[i += 1] = balancingOn << 1;
i = PackInt(i+=1, M1_Speed_CMD); i = PackInt(i += 1, M1_Speed_CMD);
i = PackInt(i, M2_Speed_CMD); i = PackInt(i, M2_Speed_CMD);
i = PackFloat(i, acc_pitch); i = PackFloat(i, acc_pitch);
i = PackFloat(i, pitch); i = PackFloat(i, pitch);
@ -103,34 +103,51 @@ void plot() {
i = PackFloat(i, TC_cont_out); i = PackFloat(i, TC_cont_out);
i = PackFloat(i, OL_cont_out); i = PackFloat(i, OL_cont_out);
i = PackFloat(i, ref_IL); i = PackFloat(i, ref_IL);
i = PackFloat(i, act_IL);
i = PackFloat(i, error_IL); i = PackFloat(i, error_IL);
i = PackFloat(i, IL_cont_out); i = PackFloat(i, IL_cont_out);
i = PackFloat(i, iError_IL); i = PackFloat(i, iError_IL);
i = PackFloat(i, IL_anti_windup); i = PackFloat(i, IL_anti_windup);
i = PackFloat(i, speedCmd1); i = PackFloat(i, speedCmd1);
i = PackFloat(i, speedCmd2); 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) { int PackInt(int _i, int value) {
data[_i] = (value & 0x00FF) ; data[_i] = (value & 0x00FF);
data[_i + 1] = (value & 0xFF00)>>8; data[_i + 1] = (value & 0xFF00) >> 8;
return _i + 2; 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 { union FloatToBytes {
float value; float value;
byte bytes[4]; byte bytes[4];
}; };
int PackFloat(int _i, float value) { int PackFloat(int _i, float value) {
FloatToBytes converter; FloatToBytes converter;
converter.value = value; converter.value = value;
for(int j = 0; j < 4; j++) { for (int j = 0; j < 4; j++) {
data[_i + j] = converter.bytes[j]; data[_i + j] = converter.bytes[j];
} }
return _i + 4; return _i + 4;
} }
// int PackFloat(int _i, float value) { // int PackFloat(int _i, float value) {
// data[_i] = (value & 0x000000FF) ; // data[_i] = (value & 0x000000FF) ;
// data[_i + 2] = (value & 0x0000FF00)>>8; // data[_i + 2] = (value & 0x0000FF00)>>8;