From 6a03848435ac16248a6f124fbc1e39a0ce6b7328 Mon Sep 17 00:00:00 2001
From: Stedd <toanpa+git@pm.me>
Date: Sun, 22 Oct 2023 01:18:13 +0200
Subject: [PATCH] Packing data for UDP telegram

Naive approach, but it works for now
---
 Balancebot.ino |  4 +++-
 UDP.ino        |  3 ---
 plot.ino       | 49 +++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 52 insertions(+), 4 deletions(-)

diff --git a/Balancebot.ino b/Balancebot.ino
index ee1d424..fae01b6 100644
--- a/Balancebot.ino
+++ b/Balancebot.ino
@@ -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();
 
diff --git a/UDP.ino b/UDP.ino
index 3c984bb..19e6b33 100644
--- a/UDP.ino
+++ b/UDP.ino
@@ -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);
 }
 
diff --git a/plot.ino b/plot.ino
index a86201b..bc73582 100644
--- a/plot.ino
+++ b/plot.ino
@@ -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;
+// }