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

Compare commits

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

No commits in common. "26955c1c123894b9cb03a061a6278128b25e03bc" and "6a03848435ac16248a6f124fbc1e39a0ce6b7328" have entirely different histories.
26955c1c12 ... 6a03848435

2 changed files with 32 additions and 48 deletions
Show Stats Expand all files Collapse all files

11
motorControl.ino
View File

@ -13,7 +13,7 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float gainScale = 1; const float gainScale = 0.75;
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
@ -65,10 +65,13 @@ void motors() {
IMU.init(); IMU.init();
} }
if (balancingOn) {
//Calculate wheel angular velocity //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[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); 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);
@ -76,12 +79,10 @@ void motors() {
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);
if (balancingOn) {
// 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);

17
plot.ino
View File

@ -103,19 +103,12 @@ 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) {
@ -124,14 +117,6 @@ int PackInt(int _i, int value) {
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];
@ -146,8 +131,6 @@ int PackFloat(int _i, float value) {
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;