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

Moved UdpPacking out of plot()

This commit is contained in:
Stedd 2023-10-22 14:10:34 +02:00
parent 0c5b26304f
commit 865eddb725
2 changed files with 60 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
UDP.ino
View File

@ -28,3 +28,63 @@ void ConnectToWiFi() {
}
}
}
void PackUdpData() {
int i = 0;
data[i] = watchdog++;
data[i += 1] = balancingOn << 1;
i = PackInt(i += 1, M1_Speed_CMD);
i = PackInt(i, M2_Speed_CMD);
i = PackFloat(i, acc_pitch);
i = PackFloat(i, pitch);
i = PackFloat(i, pitch_rate);
i = PackFloat(i, rem_speed_ref);
i = PackFloat(i, rem_turn_speed_ref);
i = PackFloat(i, SC_cont_out);
i = PackFloat(i, TC_cont_out);
i = PackFloat(i, OL_cont_out);
i = PackFloat(i, ref_IL);
i = PackFloat(i, act_IL);
i = PackFloat(i, error_IL);
i = PackFloat(i, IL_cont_out);
i = PackFloat(i, iError_IL);
i = PackFloat(i, IL_anti_windup);
i = PackFloat(i, speedCmd1);
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) {
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];
};
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;
}

56
plot.ino
View File

@ -88,62 +88,6 @@ void SerialPlot() {
// Serial.print("ry:");
// Serial.println(Ps3.data.analog.stick.ry);
// }
int i = 0;
data[i] = watchdog++;
data[i += 1] = balancingOn << 1;
i = PackInt(i += 1, M1_Speed_CMD);
i = PackInt(i, M2_Speed_CMD);
i = PackFloat(i, acc_pitch);
i = PackFloat(i, pitch);
i = PackFloat(i, pitch_rate);
i = PackFloat(i, rem_speed_ref);
i = PackFloat(i, rem_turn_speed_ref);
i = PackFloat(i, SC_cont_out);
i = PackFloat(i, TC_cont_out);
i = PackFloat(i, OL_cont_out);
i = PackFloat(i, ref_IL);
i = PackFloat(i, act_IL);
i = PackFloat(i, error_IL);
i = PackFloat(i, IL_cont_out);
i = PackFloat(i, iError_IL);
i = PackFloat(i, IL_anti_windup);
i = PackFloat(i, speedCmd1);
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) {
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];
};
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;
}