From c6f93039a164fcfe5604d64cfc6300d97307b04d Mon Sep 17 00:00:00 2001
From: Mee2001 <victor01.s@live.co.uk>
Date: Mon, 14 Jun 2021 20:32:07 +0100
Subject: [PATCH] IMU Stuff

---
 Drive/src/main.cpp |  5 ++---
 IMU/platformio.ini |  3 +++
 IMU/src/main.cpp   | 12 +++++++++---
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp
index 750d3fb..da06bdf 100644
--- a/Drive/src/main.cpp
+++ b/Drive/src/main.cpp
@@ -484,7 +484,6 @@ void loop()
     if (!initialAngleSet)
     {
       //turn to angle
-      currentHeading = getCurrentHeading();
       if (currentHeading < requiredHeading)
       { //turn right
         Serial.println("turning right");
@@ -647,12 +646,12 @@ void loop()
   total_x = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch)
   total_y = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch)
 
-  Serial.print('\n');
+/*   Serial.print('\n');
 
   Serial.println("Distance_x = " + String(total_x));
 
   Serial.println("Distance_y = " + String(total_y));
-  Serial.print('\n');
+  Serial.print('\n'); */
   //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
   unsigned long currentMillis = millis();
   if (loopTrigger)
diff --git a/IMU/platformio.ini b/IMU/platformio.ini
index ac15fe7..f89137d 100644
--- a/IMU/platformio.ini
+++ b/IMU/platformio.ini
@@ -15,4 +15,7 @@ framework = arduino
 lib_deps = 
 	tanakamasayuki/I2C MPU6886 IMU@^1.0.0
 	m5stack/M5StickC@^0.2.0
+	bblanchon/ArduinoJson@^6.18.0
 monitor_speed = 115200
+monitor_port = COM8
+upload_port = COM8
diff --git a/IMU/src/main.cpp b/IMU/src/main.cpp
index 96031d8..c04011f 100644
--- a/IMU/src/main.cpp
+++ b/IMU/src/main.cpp
@@ -1,5 +1,6 @@
 #include <Arduino.h>
 #include <M5StickC.h>
+#include <ArduinoJson.h>
 
 float pitch, roll, yaw;
 
@@ -7,13 +8,18 @@ void setup() {
   M5.begin();
   M5.IMU.Init();
   Serial.begin(115200);
-  Serial1.begin(115200, SERIAL_8N1, 26, 0);
+  Serial1.begin(9600, SERIAL_8N1, 26, 0);
 }
 void loop() {
   M5.IMU.getAhrsData(&pitch, &roll, &yaw);
   M5.Lcd.setCursor(0, 45);
   M5.Lcd.printf("%5.2f\n", roll);
-  Serial.printf("{\"cH\":%5.2f}\n", roll);
-  Serial1.printf("{\"cH\":%5.2f}\n", roll);
+  // Serial.printf("{\"cH\":%5.2f}\n", roll);
+  // Serial1.printf("{\"cH\":%5.2f}\n", roll);
+  DynamicJsonDocument tdoc(1024);
+  tdoc["cH"] = roll;
+	serializeJson(tdoc, Serial1);
+
+	
   delay(50);
 }
\ No newline at end of file