From 72021e7208b4b0c7c82a534c3ae42cef7bd73317 Mon Sep 17 00:00:00 2001
From: Aadi Desai <21363892+supleed2@users.noreply.github.com>
Date: Tue, 15 Jun 2021 01:33:52 +0100
Subject: [PATCH] Fix Instruction Queue and Emergency Stop

---
 Control/src/main.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/Control/src/main.cpp b/Control/src/main.cpp
index 5a80e17..ff2cb42 100644
--- a/Control/src/main.cpp
+++ b/Control/src/main.cpp
@@ -195,6 +195,7 @@ void loop()
 			break;
 			}
 			lastExecutedCommand = instr->id; // Update tracker of last processed command
+			InstrQueue.pop();
 		}
 	}
 	break;
@@ -439,10 +440,7 @@ void recvFromCompass()
 void emergencyStop()
 {
 	DynamicJsonDocument tdoc(1024);
-	tdoc["rH"] = heading;
-	tdoc["dist"] = -1;
-	tdoc["sp"] = -1;
-	tdoc["cH"] = heading;
+	tdoc["stp"] = 1;
 	serializeJson(tdoc, Serial1); // Send stop signals to Drive
 	sendToEnergy(0);			  // Send stop signal to Energy
 	while (InstrQueue.size())