From 0483397ae61855325a486e7ed5741f1557ff8aa2 Mon Sep 17 00:00:00 2001
From: Aadi Desai <21363892+supleed2@users.noreply.github.com>
Date: Sun, 13 Jun 2021 17:26:50 +0100
Subject: [PATCH] Add emergency stop function

---
 Control/ref/drive.cpp |  4 ++--
 Control/src/main.cpp  | 12 ++++++++++--
 2 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/Control/ref/drive.cpp b/Control/ref/drive.cpp
index e10695f..a04c7cd 100644
--- a/Control/ref/drive.cpp
+++ b/Control/ref/drive.cpp
@@ -18,8 +18,8 @@ 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"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored
-    int distance = rdoc["dist"];
-    float speed = rdoc["sp"];
+    int distance = rdoc["dist"]; // -1 for emergency stop
+    float speed = rdoc["sp"]; // -1 for emergency stop
     int currentHeading = rdoc["cH"];
 	bool resetDistanceTravelled = rdoc["rstD"];
 
diff --git a/Control/src/main.cpp b/Control/src/main.cpp
index 637e9d8..b592441 100644
--- a/Control/src/main.cpp
+++ b/Control/src/main.cpp
@@ -357,12 +357,20 @@ void recvFromVision() // TO DO
 {
 }
 
-void emergencyStop() // TO DO
+void emergencyStop()
 {
-	// Send stop signals to drive, energy
+	DynamicJsonDocument tdoc(1024);
+	tdoc["rH"] = heading;
+	tdoc["dist"] = -1;
+	tdoc["sp"] = -1;
+	tdoc["cH"] = heading;
+	tdoc["ch"] = 0;
+	serializeJson(tdoc, Serial1); // Send stop signals to Drive
+	serializeJson(tdoc, Serial2); // Send stop signals to Energy
 	while (InstrQueue.size())
 	{
 		InstrQueue.pop(); // Clear Instruction Queue
 	}
+	Status = CS_IDLE; // Reset rover to idle state
 	Serial.println("Instruction Queue cleared");
 }
\ No newline at end of file