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

Packing data for UDP telegram 

Naive approach, but it works for now
This commit is contained in:
Stedd 2023-10-22 01:18:13 +02:00
parent 7ab1ee525d
commit 6a03848435
3 changed files with 52 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Balancebot.ino
View File

@ -43,6 +43,9 @@ volatile bool M2_A_state, M2_B_state;
//PS3 Controller variables
const char* _ps3Address = "18:5e:0f:92:00:6c";
//UDP variables
uint8_t data[30 * 4];
void setup() {
//Initialize serial
Serial.begin(9600);
@ -105,7 +108,6 @@ void loop() {
//Control motors
motors();
// Plot
plot();

3
UDP.ino
View File

@ -8,7 +8,6 @@ const IPAddress multicastIP = IPAddress(239, 1, 2, 3);
int port = 1234;
byte watchdog = 0;
AsyncUDP udp;
void UdpInit() {
@ -18,8 +17,6 @@ void UdpInit() {
}
void UdpLoop() {
byte data[8] = { 1, 2, 3, 4, 5, 6, 7, 8 };
data[0] = watchdog++;
udp.writeTo(data, sizeof(data), multicastIP, port);
}

49
plot.ino
View File

@ -88,4 +88,53 @@ void plot() {
// 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, 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);
}
int PackInt(int _i, int value) {
data[_i] = (value & 0x00FF) ;
data[_i + 1] = (value & 0xFF00)>>8;
return _i + 2;
}
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;
}
// int PackFloat(int _i, float value) {
// data[_i] = (value & 0x000000FF) ;
// data[_i + 2] = (value & 0x0000FF00)>>8;
// data[_i + 3] = (value & 0x00FF0000)>>16;
// data[_i + 4] = (value & 0xFF000000)>>24;
// return _i + 4;
// }