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 //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 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 //Help variables
int M1_Speed_CMD, M2_Speed_CMD; int M1_Speed_CMD, M2_Speed_CMD;
@ -62,7 +62,7 @@ void motors() {
} }
if (Ps3.data.button.square) { if (Ps3.data.button.square) {
IMU.init(); IMU.init();
} }
@ -82,7 +82,6 @@ void motors() {
// 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);

30
plot.ino
View File

@ -119,25 +119,35 @@ void plot() {
} }
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;