Merge branch 'main' into Integration

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

Control/include/instruction.h

@@ -5,7 +5,9 @@ typedef enum {
     INSTR_RESET = -1,
     INSTR_STOP,
     INSTR_MOVE,
-    INSTR_CHARGE
+    INSTR_CHARGE,
+    INSTR_WAIT,
+    INSTR_COLOUR
 } instr_t;
 
 typedef struct instruction
@@ -16,6 +18,8 @@ typedef struct instruction
     int distance;
     float speed;
     int charge;
+    int time;
+    int colour;
 } RoverInstruction;
 
 #endif

Control/include/status.h

@@ -5,7 +5,8 @@ typedef enum {
     CS_ERROR = -1,
     CS_IDLE,
     CS_MOVING,
-    CS_CHARGING
+    CS_CHARGING,
+    CS_WAITING
 } ControlStatus_t;
 
 #endif

Control/ref/command.cpp

@@ -5,7 +5,7 @@
 #define WebSocket 0
 int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command
 float batteryVoltage, batteryLevel, batteryCycles; // Info Control ==> Command
-int command_id, mode, reqHeading, reqDistance, reqCharge; // Info Command ==> Control
+int command_id, mode, reqHeading, reqDistance, reqCharge, reqTime, reqColour; // Info Command ==> Control
 float reqSpeed; // Info Command ==> Control
 
 void setup() {}
@@ -14,7 +14,7 @@ void loop()
 {
 	DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
 	deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket
-	state = rdoc["st"]; // State: -1 = Error, 0 = Idle, 1 = Moving, 2 = Charging
+	state = rdoc["st"]; // State: -1 = Error, 0 = Idle, 1 = Moving, 2 = Charging, 3 = Waiting
 	batteryVoltage = rdoc["bV"];
 	batteryLevel = rdoc["bL"];
 	batteryCycles = rdoc["bC"];
@@ -36,9 +36,13 @@ void loop()
 							//  0 = Stop immediately, clear command cache
 							//  1 = Normal movement command, added to end of command cache
 							//  2 = Normal charge command, results in no motion, added to end of command cache
+							//  3 = Pause command, wait for defined time in seconds, added to end of command cache
+							//  4 = Change colour tracking using Vision
 	tdoc["rH"] = reqHeading;
 	tdoc["rD"] = reqDistance;
 	tdoc["rS"] = reqSpeed;
 	tdoc["rC"] = reqCharge;
+	tdoc["pSt"] = reqTime;
+	tdoc["col"] = reqColour;
 	serializeJson(tdoc, WebSocket, WebSocket); // Build JSON and send on UART1
 }

Control/src/main.cpp

@@ -13,6 +13,7 @@
 #include <SPIFFS.h>
 #include "status.h"
 #include "instruction.h"
+#include "colour.h"
 #include <queue>
 #pragma endregion
 
@@ -45,6 +46,7 @@ void recvFromEnergy();
 void sendToVision();
 void recvFromVision();
 void recvFromCompass();
+void updateRSSI();
 void emergencyStop();
 #pragma endregion
 
@@ -70,6 +72,8 @@ bool driveCommandComplete;
 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()
@@ -98,6 +102,8 @@ void setup()
 	lastCompletedCommand = 0;
 	driveCommandComplete = 1;
 	chargeGoal = 0;
+	waitGoal = 0;
+	colour = C_RED;
 
 	if (!SPIFFS.begin(true)) // Mount SPIFFS
 	{
@@ -139,6 +145,7 @@ void loop()
 	recvFromEnergy();		// Update stats from Energy
 	// recvFromVision();		// Update stats from Vision
 	recvFromCompass();		// Update stats from Compass
+	updateRSSI();
 	switch (Status)
 	{
 	case CS_ERROR:
@@ -188,6 +195,18 @@ void loop()
 				sendToEnergy(1);				   // Forward to Energy handler
 			}
 			break;
+			case INSTR_WAIT: // Normal wait
+			{
+				Status = CS_WAITING;						// Set waiting state
+				waitGoal = millis() + 1000 * (instr->time); // Set wait time
+			}
+			break;
+			case INSTR_COLOUR:
+			{
+				Status = CS_IDLE;
+				colour = (Colour_t)instr->colour;
+			}
+			break;
 			default:
 			{
 				Serial.println("Unknown instruction type in queue, skipping...");
@@ -226,6 +245,16 @@ void loop()
 		// Otherwise continue charging, no change
 	}
 	break;
+	case CS_WAITING:
+	{
+		if (millis() >= waitGoal) // Compare waitGoal to current time
+		{
+			Status = CS_IDLE;
+			lastCompletedCommand = lastExecutedCommand; // Update last completed command
+		}
+		// Otherwise continue waiting, no change
+	}
+	break;
 	default:
 	{
 		Serial.println("Unknown rover state, exiting...");
@@ -279,7 +308,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 			Serial.println("Reset telemetry command received");
 			instr.id = rdoc["Cid"];
 			instr.instr = INSTR_RESET;
-			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"], rdoc["pSt"]
 
 			queueInstruction(instr); // Put reset command in InstrQueue
 		}
@@ -288,7 +317,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 		{
 			Serial.println("Emergency stop command received");
 			// instr.instr = INSTR_STOP; // Not needed as Emergency Stop is not queued
-			// Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+			// Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"], rdoc["pSt"]
 
 			emergencyStop();
 		}
@@ -301,7 +330,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 			instr.heading = rdoc["rH"];
 			instr.distance = rdoc["rD"];
 			instr.speed = rdoc["rS"];
-			// Ignore rdoc["rC"]
+			// Ignore rdoc["rC"], rdoc["pSt"]
 
 			queueInstruction(instr); // Put movement command in InstrQueue
 		}
@@ -312,7 +341,29 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 			instr.id = rdoc["Cid"];
 			instr.instr = INSTR_CHARGE;
 			instr.charge = rdoc["rC"];
-			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"]
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["pSt"]
+
+			queueInstruction(instr); // Put charge command in InstrQueue
+		}
+		break;
+		case 3: // Normal wait command, results in no motion, added to end of command cache
+		{
+			Serial.println("Normal wait command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_WAIT;
+			instr.time = rdoc["pSt"];
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+
+			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
 		}
@@ -408,7 +459,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
@@ -437,6 +488,11 @@ void recvFromCompass()
 	}
 }
 
+void updateRSSI()
+{
+	signalStrength = WiFi.RSSI();
+}
+
 void emergencyStop()
 {
 	DynamicJsonDocument tdoc(1024);