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

Formatting

This commit is contained in:
Stedd 2023-10-22 02:09:46 +02:00
parent 1fa5f97b25
commit 26955c1c12
2 changed files with 27 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
motorControl.ino
View File

@ -13,13 +13,13 @@ const float DEADBAND_M2_NEG = 90.0;
//Tuning
const float K_SC = 18.5*gainScale; //Speed 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_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 gainScale = 1;
const float K_SC = 18.5 * gainScale; //Speed 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_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
//Help variables
int M1_Speed_CMD, M2_Speed_CMD;
@ -62,7 +62,7 @@ void motors() {
}
if (Ps3.data.button.square) {
IMU.init();
IMU.init();
}
@ -82,7 +82,6 @@ void motors() {
// Speed Controller
SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
// Balance controller
// Outer loop
OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);

30
plot.ino
View File

@ -119,25 +119,35 @@ void plot() {
}
int PackInt(int _i, int value) {
data[_i] = (value & 0x00FF) ;
data[_i + 1] = (value & 0xFF00)>>8;
data[_i] = (value & 0x00FF);
data[_i + 1] = (value & 0xFF00) >> 8;
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 {
float value;
byte bytes[4];
float value;
byte bytes[4];
};
int PackFloat(int _i, float value) {
FloatToBytes converter;
converter.value = value;
for(int j = 0; j < 4; j++) {
data[_i + j] = converter.bytes[j];
}
return _i + 4;
FloatToBytes converter;
converter.value = value;
for (int j = 0; j < 4; j++) {
data[_i + j] = converter.bytes[j];
}
return _i + 4;
}
// int PackFloat(int _i, float value) {
// data[_i] = (value & 0x000000FF) ;
// data[_i + 2] = (value & 0x0000FF00)>>8;