3 files changed, 72 insertions, 0 deletions
diff --git a/debug/IMU_comp/Calibrate/Calibrate.ino b/debug/IMU_comp/Calibrate/Calibrate.ino
new file mode 100644
index 0000000..c5db31c
--- /dev/null
+++ b/debug/IMU_comp/Calibrate/Calibrate.ino
@@ -0,0 +1,42 @@
+//example from https://github.com/pololu/lis3mdl-arduino
+#include <Wire.h>
+#include <LIS3MDL.h>
+
+LIS3MDL mag;
+LIS3MDL::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};
+
+char report[80];
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!mag.init())
+  {
+    Serial.println("Failed to detect and initialize magnetometer!");
+    while (1);
+  }
+
+  mag.enableDefault();
+}
+
+void loop()
+{
+  mag.read();
+
+  running_min.x = min(running_min.x, mag.m.x);
+  running_min.y = min(running_min.y, mag.m.y);
+  running_min.z = min(running_min.z, mag.m.z);
+
+  running_max.x = max(running_max.x, mag.m.x);
+  running_max.y = max(running_max.y, mag.m.y);
+  running_max.z = max(running_max.z, mag.m.z);
+
+  snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d}   max: {%+6d, %+6d, %+6d}",
+    running_min.x, running_min.y, running_min.z,
+    running_max.x, running_max.y, running_max.z);
+  Serial.println(report);
+
+  delay(100);
+}
diff --git a/debug/IMU_comp/IMU_comp.ino b/debug/IMU_comp/IMU_comp.ino
index fc35244..c4578cb 100644
--- a/debug/IMU_comp/IMU_comp.ino
+++ b/debug/IMU_comp/IMU_comp.ino
@@ -65,6 +65,9 @@ void loop()
 
   float heading = computeHeading();
   int offset = front - heading;
+  if(offset<0){
+    offset=360+offset;
+  }
   
   Serial.print("heading: ");
   Serial.print(heading);
diff --git a/debug/IMU_comp/calcoffset/calcoffset.ino b/debug/IMU_comp/calcoffset/calcoffset.ino
new file mode 100644
index 0000000..fc5f752
--- /dev/null
+++ b/debug/IMU_comp/calcoffset/calcoffset.ino
@@ -0,0 +1,27 @@
+void setup() {
+  randomSeed(analogRead(0));
+  Serial.begin(9600);
+
+}
+
+void loop() {
+  int front = random(0, 360);
+  int heading = random(0, 360);
+  Serial.print("Front: ");
+  Serial.println(front);
+  Serial.print("heading: ");
+  Serial.println(heading);
+
+  int offset = heading - front;
+  if(offset<0){
+    offset=(offset+360);
+  }
+  Serial.print("offset: ");
+  Serial.println(offset);
+  
+  Serial.println();
+  Serial.println();
+
+  delay(5000);
+
+}