Rust Visualisation (Draft)

.gitignore

@@ -1,2 +1,4 @@
 build
 .vscode/c_cpp_properties.json
+visualisation/target
+Simulations/venv

BallSensors.h

@@ -173,6 +173,12 @@ class BallSensors
     return Heading(strongestSensorAngle * DEG_TO_RAD);
   }
 
+  float *getSensorValues()
+  {
+    updateSensorReadings();
+    return sensorValues;
+  }
+
   /// @brief Streams sensor data as CSV over Serial.
   /// The output format is:
   /// reading1,reading2,...,reading8,angle1,angle2,...,angle8,ball_heading,detected_flag

FutBot.ino

@@ -15,8 +15,10 @@
 #include "Strategy.h"
 #include "Sonar.h"
 #include "ColourSensor.h"
+#include "Json.h"
 #include <Wire.h>
 
+#define VISUALIZATION
 #define PRINT_CTX
 
 // Compass Calibration Parameters
@@ -27,6 +29,7 @@ const int BLUE_SIGNATURE = 2;
 
 // Start-of-game Calibration Parameters
 Heading downfield_heading = Heading(0);
+
 bool yellowwards = true;  // is the opp goal yellow?
 
 // Peripheral objects
@@ -110,6 +113,10 @@ void DoCalibration() {
 void loop() {
   Context ctx = UpdateSensors();
 
+#ifdef VISUALIZATION
+  SendJson();
+#endif // VISUALIZATION
+
 #ifdef PRINT_CTX
   ctx.SerialPrint();
   Serial.println();
@@ -174,4 +181,20 @@ Context UpdateSensors() {
 
 
   return ctx;
+}
+
+void SendJson()
+{
+  Readings readings = {
+      .translational = {},
+      .translational_count = 0,
+      .rotational = {},
+      .rotational_count = 0,
+      .motors = {.fl = 0, .fr = 0, .r = 0},
+      .compass = ctx.compass.as_degrees(),
+      .color = {.r = 0, .g = 0, .b = 0},
+      .ir = ball_sensors->getSensorValues(),
+  };
+
+  SendReadingsJson(readings);
 }

Json.h

@@ -0,0 +1,83 @@
+#pragma once
+#ifndef JSON_H
+#define JSON_H
+
+#include <ArduinoJson.h>
+
+const int VECTOR_SIZE = 5;
+
+struct Readings
+{
+    struct Translational
+    {
+        float x;
+        float y;
+        const char *source;
+    } translational[VECTOR_SIZE];
+    int translational_count;
+
+    struct Rotational
+    {
+        float val;
+        const char *source;
+    } rotational[VECTOR_SIZE];
+    int rotational_count;
+
+    struct Motors
+    {
+        float fl;
+        float fr;
+        float r;
+    } motors;
+
+    float compass;
+
+    struct Color
+    {
+        int r;
+        int g;
+        int b;
+    } color;
+
+    float *ir;
+};
+
+/// @brief Send readings over serial in JSON
+/// @param readings The readings to send
+void SendReadingsJson(Readings &readings)
+{
+    JsonDocument doc;
+
+    doc["compass"] = readings.compass;
+
+    doc["color"]["r"] = readings.color.r;
+    doc["color"]["g"] = readings.color.g;
+    doc["color"]["b"] = readings.color.b;
+
+    doc["motors"]["fl"] = readings.motors.fl;
+    doc["motors"]["fr"] = readings.motors.fr;
+    doc["motors"]["r"] = readings.motors.r;
+
+    for (int i = 0; i < 8; i++)
+    {
+        doc["ir"][i] = readings.ir[i];
+    }
+
+    for (int i = 0; i < readings.translational_count; i++)
+    {
+        doc["translational"][i]["x"] = readings.translational[i].x;
+        doc["translational"][i]["y"] = readings.translational[i].y;
+        doc["translational"][i]["source"] = readings.translational[i].source;
+    }
+
+    for (int i = 0; i < readings.rotational_count; i++)
+    {
+        doc["rotational"][i]["val"] = readings.rotational[i].val;
+        doc["rotational"][i]["source"] = readings.rotational[i].source;
+    }
+
+    serializeJson(doc, Serial);
+    Serial.println();
+}
+
+#endif