From 9463a14cdcb077003c30d00896199d38810e0720 Mon Sep 17 00:00:00 2001
From: Aadi Desai <21363892+supleed2@users.noreply.github.com>
Date: Mon, 7 Jun 2021 14:08:58 +0100
Subject: [PATCH] Create Drive<->Control reference doc

---
 Control/ref/drive.cpp | 40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)
 create mode 100644 Control/ref/drive.cpp

diff --git a/Control/ref/drive.cpp b/Control/ref/drive.cpp
new file mode 100644
index 0000000..5658baf
--- /dev/null
+++ b/Control/ref/drive.cpp
@@ -0,0 +1,40 @@
+#include <Arduino.h>
+#include <ArduinoJson.h>
+#include <string>
+
+#define RXpin 0 // Define your RX pin here
+#define TXpin 0 // Define your TX pin here
+
+void setup()
+{
+    Serial.begin(115200);                          // Set up hardware UART0 (Connected to USB port)
+    Serial1.begin(9600, SERIAL_8N1, RXpin, TXpin); // Set up hardware UART1
+
+    // Other Drive setup stuff
+}
+
+void loop()
+{
+    DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
+    deserializeJson(rdoc, Serial1); // Take JSON input from UART1
+    int requiredHeading = rdoc["rH"];
+    int distance = rdoc["dist"];
+    float speed = rdoc["sp"];
+    int currentHeading = rdoc["cH"];
+
+    bool commandComplete = 0;
+    float powerUsage_mW = 0.0;
+    int distTravelled_mm = 0;
+    int current_x = 0;
+    int current_y = 0;
+
+    // Do Drive stuff, set the 5 values above
+
+    DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
+    tdoc["comp"] = commandComplete;
+    tdoc["mW"] = powerUsage_mW;
+    tdoc["mm"] = distTravelled_mm;
+    tdoc["pos"][0] = current_x;
+    tdoc["pos"][1] = current_y;
+    serializeJson(tdoc, Serial1); // Build JSON and send on UART1
+}
\ No newline at end of file