From c98bafd6f77365c45ed09ed2be80429e851b53f9 Mon Sep 17 00:00:00 2001
From: Aadi Desai <21363892+supleed2@users.noreply.github.com>
Date: Tue, 15 Jun 2021 03:14:04 +0100
Subject: [PATCH] Add ability to select Vision colour tracking

---
 Control/include/colour.h      | 12 ++++++++++++
 Control/include/instruction.h |  4 +++-
 Control/src/main.cpp          | 22 +++++++++++++++++++++-
 3 files changed, 36 insertions(+), 2 deletions(-)
 create mode 100644 Control/include/colour.h

diff --git a/Control/include/colour.h b/Control/include/colour.h
new file mode 100644
index 0000000..6544242
--- /dev/null
+++ b/Control/include/colour.h
@@ -0,0 +1,12 @@
+#ifndef COLOUR_H
+#define COLOUR_H
+
+typedef enum {
+    C_RED,
+    C_LBLUE,
+    C_GREEN,
+    C_PINK,
+    C_ORANGE
+} Colour_t;
+
+#endif
diff --git a/Control/include/instruction.h b/Control/include/instruction.h
index 1a2d32d..eed1b92 100644
--- a/Control/include/instruction.h
+++ b/Control/include/instruction.h
@@ -6,7 +6,8 @@ typedef enum {
     INSTR_STOP,
     INSTR_MOVE,
     INSTR_CHARGE,
-    INSTR_WAIT
+    INSTR_WAIT,
+    INSTR_COLOUR
 } instr_t;
 
 typedef struct instruction
@@ -18,6 +19,7 @@ typedef struct instruction
     float speed;
     int charge;
     int time;
+    Colour_t colour;
 } RoverInstruction;
 
 #endif
diff --git a/Control/src/main.cpp b/Control/src/main.cpp
index 1ad0ac4..06a9a49 100644
--- a/Control/src/main.cpp
+++ b/Control/src/main.cpp
@@ -13,6 +13,7 @@
 #include <SPIFFS.h>
 #include "status.h"
 #include "instruction.h"
+#include "colour.h"
 #include <queue>
 #pragma endregion
 
@@ -71,6 +72,7 @@ int bb_left, bb_right, bb_top, bb_bottom;
 int bb_centre_x, bb_centre_y;
 float chargeGoal;
 int waitGoal;
+Colour_t colour;
 #pragma endregion
 
 void setup()
@@ -100,6 +102,7 @@ void setup()
 	driveCommandComplete = 1;
 	chargeGoal = 0;
 	waitGoal = 0;
+	colour = C_RED;
 
 	if (!SPIFFS.begin(true)) // Mount SPIFFS
 	{
@@ -196,6 +199,12 @@ void loop()
 				waitGoal = millis() + 1000*(instr->time); // Set wait time
 			}
 			break;
+			case INSTR_COLOUR:
+			{
+				Status = CS_IDLE;
+				colour = instr->colour;
+			}
+			break;
 			default:
 			{
 				Serial.println("Unknown instruction type in queue, skipping...");
@@ -346,6 +355,17 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 			queueInstruction(instr); // Put charge command in InstrQueue
 		}
 		break;
+		case 4: // Normal wait command, results in no motion, added to end of command cache
+		{
+			Serial.println("Change colour tracking command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_COLOUR;
+			instr.colour = rdoc["col"];
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+
+			queueInstruction(instr); // Put charge command in InstrQueue
+		}
+		break;
 		default:
 		{
 			// Default case, print and continue
@@ -437,7 +457,7 @@ void recvFromEnergy() // Update telemetry data and state info from Energy packet
 
 void sendToVision()
 {
-	Serial3.print("R"); // Request new data from Vision
+	Serial3.print(colour); // Select coloured ball to track
 }
 
 void recvFromVision() // Update bounding box and obstacle detection data from Vision packet