Set up basic rover flow, individual functions to be completed

Control/src/main.cpp

@@ -1,7 +1,7 @@
 #pragma region Includes
 #include <Arduino.h>
 #include <string>
-// #include <SoftwareSerial.h> Software Serial not currently needed
+#include <SoftwareSerial.h>
 #include <AsyncTCP.h>
 #include <ESPAsyncWebServer.h>
 #include <ESPmDNS.h>
@@ -12,6 +12,7 @@
 #include <SPIFFS.h>
 #include "status.h"
 #include "instruction.h"
+#include <queue>
 #pragma endregion
 
 #pragma region Enable extra debugging info for ESP32
@@ -21,27 +22,44 @@
 #pragma endregion
 
 #pragma region Definitions eg pins
-#define RX1pin 14 // Pin 10 on expansion board, UART1
+#define RX1pin 18 // Pin 6 on expansion board, UART1
-#define TX1pin 4  // Pin 11 on expansion board, UART1
+#define TX1pin 5  // Pin 7 on expansion board, UART1
+#define RX2pin 17 // Pin 8 on expansion board, UART2
+#define TX2pin 16 // Pin 9 on expansion board, UART2
+#define RX3pin 14 // Pin 10 on expansion board, UART3
+#define TX3pin 4  // Pin 11 on expansion board, UART3
 #pragma endregion
 
 #pragma region Function Declarations
-void printFPGAoutput();
-void returnSensorData();
 void notFound(AsyncWebServerRequest *request);
 void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
+void queueInstruction(RoverInstruction instruction);
+void sendToCommand();
+void sendToDrive(RoverInstruction instruction);
+void recvFromDrive();
+void sendToEnergy(RoverInstruction instruction);
+void recvFromEnergy();
+void sendToVision();
+void recvFromVision();
 #pragma endregion
 
 #pragma region Global objects
 AsyncWebServer webserver(80);
 WebSocketsServer websocketserver(81);
 Ticker ticker;
+SoftwareSerial Serial3;
+std::queue<RoverInstruction> InstrQueue;
 #pragma endregion
 
 #pragma region Global variables
 ControlStatus_t Status;
-float battery_voltage = 4.0f;
+float batteryVoltage;
-int distance_travelled = 0;
+int batteryLevel;
+int odometer;
+int heading;
+int xpos, ypos;
+int signalStrength;
+int lastCompletedCommand;
 #pragma endregion
 
 void setup()
@@ -50,10 +68,21 @@ void setup()
 	esp_log_level_set("wifi", ESP_LOG_WARN);  // enable WARN logs from WiFi stack
 	esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client
 
+	Serial.begin(115200);							   // Set up hardware UART0 (Connected to USB port)
+	Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin);   // Set up hardware UART1 (Connected to Drive)
+	Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin);   // Set up hardware UART2 (Connected to Energy)
+	Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision)
+
+	// Set global variable startup values
 	Status = CS_IDLE;
-	Serial.begin(115200);							 // Set up hardware UART0 (Connected to USB port)
+	batteryVoltage = 0;
-	Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1
+	batteryLevel = 0;
-	// Set up remaining communication ports here (Energy, Drive, Vision)
+	odometer = 0;
+	heading = 0;
+	xpos = 0;
+	ypos = 0;
+	signalStrength = 0;
+	lastCompletedCommand = 0;
 
 	if (!SPIFFS.begin(true)) // Mount SPIFFS
 	{
@@ -63,11 +92,11 @@ void setup()
 	Serial.println("SPIFFS mounted");
 
 	WiFi.begin(WIFI_SSID, WIFI_PW);
-	while (WiFi.status() != WL_CONNECTED)
+	while (WiFi.status() != WL_CONNECTED) // Wait for ESP32 to connect to AP in "credentials.h"
 	{
 		delay(500);
 	}
-	while (!MDNS.begin("rover"))
+	while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/"
 	{
 		Serial.println("Error setting up mDNS, retrying in 5s");
 		delay(5000);
@@ -75,56 +104,95 @@ void setup()
 	Serial.println("mDNS set up, access Control Panel at 'rover.local/'");
 
 	webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
-				 { request->send(SPIFFS, "/index.html", "text/html"); });
+				 { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
 	webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
-				 { request->send(SPIFFS, "/favicon.ico", "image/png"); });
+				 { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
-	webserver.onNotFound(notFound);
+	webserver.onNotFound(notFound);										   // Set up basic 404NotFound page
-	webserver.begin();
+	webserver.begin();													   // Start Asynchronous Web Server
 
-	websocketserver.begin();
+	websocketserver.begin();				 // Start Websocket Server
-	websocketserver.onEvent(webSocketEvent);
+	websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command
-	ticker.attach(0.5, returnSensorData);
+	ticker.attach(0.5, sendToCommand);		 // Set up recurring function to forward rover status to Command
 }
 
-void loop()
+void loop() // TO DO
 {
-	printFPGAoutput();
-
-	String FPGAinput; // Forward serial monitor input to FPGA
-	if (Serial.available())
-	{
-		FPGAinput = String(Serial.readStringUntil('\n'));
-		Serial1.println(FPGAinput);
-	}
-
 	websocketserver.loop(); // Handle incoming client connections
-}
+	switch (Status)
-
-void printFPGAoutput()
-{ // Print serial communication from FPGA to serial monitor
-	String FPGAoutput;
-	if (Serial1.available())
 	{
-		FPGAoutput = String(Serial1.readStringUntil('\n'));
+	case CS_ERROR:
-		Serial.println(FPGAoutput);
-	}
-}
-
-void returnSensorData()
-{
-	// Collect sensor data here?
-	distance_travelled++;
-	if (battery_voltage < 6)
 	{
-		battery_voltage += 0.2;
+		Serial.println("Rover in error state, rebooting...");
+		exit(1);
 	}
-	else
+	break;
+	case CS_IDLE:
 	{
-		battery_voltage = 4;
+		if (InstrQueue.empty()) // If Rover idle and InstrQueue empty:
+		{
+			// TO DO: Collect all data (recvFrom) and
+			sendToCommand(); // Update command panel
+							 // Maybe wait 1s? Possibly prevent from looping too fast
+		}
+		else
+		{
+			// Do the next command in the queue
+			RoverInstruction *instr = &InstrQueue.front();
+			switch (instr->instr)
+			{
+			case INSTR_RESET:
+			{
+				odometer = 0;
+				xpos = 0;
+				ypos = 0;
+			}
+			break;
+			case INSTR_STOP:
+			{
+				while (1)
+				{
+					Serial.println("Emergency Stop should not get queued, hold and print");
+					delay(1000);
+				}
+			}
+			break;
+			case INSTR_MOVE:
+			{
+				Status = CS_MOVING;
+				sendToDrive(*instr);
+			}
+			break;
+			case INSTR_CHARGE:
+			{
+				Status = CS_CHARGING;
+				sendToEnergy(*instr);
+			}
+			break;
+			default:
+			{
+				Serial.println("Unknown instruction type in queue, skipping...");
+			}
+			break;
+			}
+		}
+	}
+	break;
+	case CS_MOVING:
+	{
+		// TO DO
+	}
+	break;
+	case CS_CHARGING:
+	{
+		// TO DO
+	}
+	break;
+	default:
+	{
+		Serial.println("Unknown rover state, exiting...");
+		exit(1);
+	}
 	}
-	String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}";
-	Serial.println(JSON_Data);
-	websocketserver.broadcastTXT(JSON_Data);
 }
 
 void notFound(AsyncWebServerRequest *request)
@@ -132,7 +200,7 @@ void notFound(AsyncWebServerRequest *request)
 	request->send(404, "text/plain", "Page Not found. Check URI/IP address.");
 }
 
-void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
+void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) // TO DO
 {
 	switch (type)
 	{
@@ -147,13 +215,13 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 		Serial.printf("Client[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
 	}
 	break;
-	case WStype_TEXT:
+	case WStype_TEXT: // MSG received from command panel
 	{
-		Serial.printf("Client[%u] sent Text: %s\n", num, payload);
+		Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal
-		String command = String((char *)(payload));
+		String command = String((char *)(payload));				   // Convert received command to string type
 
-		DynamicJsonDocument doc(200);								//creating an instance of a DynamicJsonDocument allocating 200bytes on the heap.
+		DynamicJsonDocument rdoc(200);								 // Create instance of DynamicJsonDocument on heap, 200 Bytes
-		DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive.
+		DeserializationError error = deserializeJson(rdoc, command); // Convert command string to JSONDocument and capture any errors
 		if (error)
 		{
 			Serial.print("deserializeJson() failed: ");
@@ -161,12 +229,62 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 			return;
 		}
 
-		int MVM_F_status = doc["MVM_F"];
+		RoverInstruction instr;
-		int MVM_L_status = doc["MVM_L"];
+		int mode = rdoc["mode"];
-		int MVM_R_status = doc["MVM_R"];
+		switch (mode)
-		int MVM_B_status = doc["MVM_B"];
+		{
+		case -1: // Add to queue, reset x/y/odometer (telemetry data)
+		{
+			Serial.println("Reset telemetry command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_RESET;
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
 
-		Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>');
+			/* Put reset command in commandFIFO */
+		}
+		break;
+		case 0: // Stop immediately, clear command cache
+		{
+			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"]
+
+			/* Clear commandFIFO */
+		}
+		break;
+		case 1: // Normal movement command, added to end of command cache
+		{
+			Serial.println("Normal movement command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_MOVE;
+			instr.heading = rdoc["rH"];
+			instr.distance = rdoc["rD"];
+			instr.speed = rdoc["rS"];
+			// Ignore rdoc["rC"]
+
+			/* Put movement command in commandFIFO */
+		}
+		break;
+		case 2: // Normal charge command, results in no motion, added to end of command cache
+		{
+			Serial.println("Normal charge command received");
+			instr.id = rdoc["Cid"];
+			instr.instr = INSTR_CHARGE;
+			instr.charge = rdoc["rC"];
+			// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"]
+
+			/* Put charge command in commandFIFO */
+		}
+		break;
+		default:
+		{
+			Serial.println("Unknown Command type received, ignoring"); // Default case, print and continue
+																	   // Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
+		}
+		break;
+		}
+
+		queueInstruction(instr);
 	}
 	break;
 	case WStype_PONG:
@@ -179,5 +297,72 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
 		Serial.println(String("Websocket received invalid event type: ") + type + String(", exiting"));
 		exit(1);
 	}
+	break;
 	}
 }
+
+void queueInstruction(RoverInstruction instruction)
+{
+	InstrQueue.push(instruction);
+}
+
+void sendToCommand()
+{
+	DynamicJsonDocument tdoc(1024);
+	tdoc["st"] = Status;
+	tdoc["bV"] = batteryVoltage;
+	tdoc["bL"] = batteryLevel;
+	tdoc["tD"] = odometer;
+	tdoc["cH"] = heading;
+	tdoc["pos"][0] = xpos;
+	tdoc["pos"][1] = ypos;
+	tdoc["rssi"] = signalStrength;
+	tdoc["LCCid"] = lastCompletedCommand;
+	String JSON_Data;
+	serializeJson(tdoc, JSON_Data);
+	websocketserver.broadcastTXT(JSON_Data);
+}
+
+void sendToDrive(RoverInstruction instruction)
+{
+	DynamicJsonDocument tdoc(1024);
+	tdoc["rH"] = instruction.heading;
+	tdoc["dist"] = instruction.distance;
+	tdoc["sp"] = instruction.speed;
+	tdoc["cH"] = heading;
+	serializeJson(tdoc, Serial1);
+}
+
+void recvFromDrive() // TO DO
+{
+}
+
+void sendToEnergy(RoverInstruction instruction)
+{
+	DynamicJsonDocument tdoc(1024);
+	tdoc["ch"] = instruction.charge;
+	serializeJson(tdoc, Serial2);
+}
+
+void recvFromEnergy() // TO DO
+{
+}
+
+void sendToVision()
+{
+	Serial3.print("R"); // Request new data from Vision
+}
+
+void recvFromVision() // TO DO
+{
+}
+
+void emergencyStop() // TO DO
+{
+	// Send stop signals to drive, energy
+	while (InstrQueue.size())
+	{
+		InstrQueue.pop(); // Clear Instruction Queue
+	}
+	Serial.println("Instruction Queue cleared");
+}