From 29efc08e743c866d1376c72c61a9a0c863ad79f4 Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Sat, 10 Jun 2017 06:58:11 -0700
Subject: [PATCH 01/10] Updated to report encoder ticks directly. That'll make
 it easier to do PID in abridge.

---
 Swarmathon_Arduino/Swarmathon_Arduino.ino | 11 +++----
 libraries/Odometry/Odometry.cpp           | 40 +++++++----------------
 libraries/Odometry/Odometry.h             | 12 ++-----
 3 files changed, 18 insertions(+), 45 deletions(-)

diff --git a/Swarmathon_Arduino/Swarmathon_Arduino.ino b/Swarmathon_Arduino/Swarmathon_Arduino.ino
index 4527b4f..8ec1fa0 100644
--- a/Swarmathon_Arduino/Swarmathon_Arduino.ino
+++ b/Swarmathon_Arduino/Swarmathon_Arduino.ino
@@ -67,7 +67,7 @@ L3G gyroscope;
 LSM303 magnetometer_accelerometer;
 LPS pressure;
 Movement move = Movement(rightSpeedPin, rightDirectionA, rightDirectionB, leftSpeedPin, leftDirectionA, leftDirectionB);
-Odometry odom = Odometry(rightEncoderA, rightEncoderB, leftEncoderA, leftEncoderB, wheelBase, wheelDiameter, cpr);
+Odometry odom = Odometry(rightEncoderA, rightEncoderB, leftEncoderA, leftEncoderB);
 Servo fingers;
 Servo wrist;
 NewPing leftUS(leftSignal, leftSignal, 330);
@@ -276,12 +276,9 @@ String updateOdom() {
   String txBuffer;
   odom.update();
 
-  txBuffer = String(odom.x) + "," +
-             String(odom.y) + "," +
-             String(odom.theta) + "," +
-             String(odom.vx) + "," +
-             String(odom.vy) + "," +
-             String(odom.vtheta);
+  txBuffer = String(odom.left) + "," +
+             String(odom.right) + "," +
+             String(odom.clock)
 
   return txBuffer;
 }
diff --git a/libraries/Odometry/Odometry.cpp b/libraries/Odometry/Odometry.cpp
index 99acbec..4f9cdd9 100644
--- a/libraries/Odometry/Odometry.cpp
+++ b/libraries/Odometry/Odometry.cpp
@@ -17,7 +17,7 @@ byte _leftEncoderBPin;
 /**
  *	Constructor arg are pins for channels A and B on right and left encoders
  **/
-Odometry::Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin, float wheelBase, float wheelDiameter, int cpr) {
+Odometry::Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin) {
     pinMode(rightEncoderAPin, INPUT);
     pinMode(rightEncoderBPin, INPUT);
     pinMode(leftEncoderAPin, INPUT);
@@ -33,39 +33,21 @@ Odometry::Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncode
     _rightEncoderBPin = rightEncoderBPin;
     _leftEncoderAPin = leftEncoderAPin;
     _leftEncoderBPin = leftEncoderBPin;
-    _wheelBase = wheelBase;
-    _wheelDiameter = wheelDiameter;
-    _cpr = cpr;
-    
-    theta = 0;
+    right = 0;
+    left = 0;
+    ts = 0;
     clock = millis();
 }
 
 void Odometry::update() {
-    //Calculate linear distance that each wheel has traveled
-    float rightWheelDistance = ((float)rightEncoderCounter / _cpr) * _wheelDiameter * PI;
-    float leftWheelDistance = ((float)leftEncoderCounter / _cpr) * _wheelDiameter * PI;
-    
-    //Calculate relative angle that robot has turned
-    float dtheta = (rightWheelDistance - leftWheelDistance) / _wheelBase;
-    //Calculate angular velocity
-    vtheta = dtheta / (millis() - clock) * 1000;
-    //Accumulate angles to calculate absolute heading
-    theta += dtheta;
-    
-    //Decompose linear distance into its component values
-    float meanWheelDistance = (rightWheelDistance + leftWheelDistance) / 2;
-    x = meanWheelDistance * cos(dtheta);
-    y = meanWheelDistance * sin(dtheta);
-    //Calculate linear velocity
-    vx = x / (millis() - clock) * 1000;
-    vy = y / (millis() - clock) * 1000;
-    
-    //Reset counters
-    rightEncoderCounter = 0;
+    // Record ticks.
+    left = leftEncoderCounter;
+    right = rightEncoderCounter;
+
     leftEncoderCounter = 0;
+    rightEncoderCounter = 0;
     
-    //Reset clock
+    // Store timestamp
     clock = millis();
 }
 
@@ -117,4 +99,4 @@ void setupPinChangeInterrupt(byte pin) {
     *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin));  // enable pin
     PCIFR  |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
     PCICR  |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
-}
\ No newline at end of file
+}
diff --git a/libraries/Odometry/Odometry.h b/libraries/Odometry/Odometry.h
index 3b7efe1..885a1ac 100644
--- a/libraries/Odometry/Odometry.h
+++ b/libraries/Odometry/Odometry.h
@@ -6,20 +6,14 @@
 class Odometry {
 public:
     //Constructors
-    Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin, float wheelBase, float wheelDiameter, int cpr);
+    Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncoderAPin, byte leftEncoderBPin);
     
     //Functions
     void update();
     
     //Variables
-    float x, y, theta;
-    float vx, vy, vtheta;
+    uint16_t right, left;
     long clock;
-
-private:
-    //Variables
-    float _wheelBase, _wheelDiameter;
-    int _cpr;
 };
 
-#endif
\ No newline at end of file
+#endif

From 5b2f005b683d8af28f2a3224283e9564db745d9a Mon Sep 17 00:00:00 2001
From: Mike Matera <mike-matera@users.noreply.github.com>
Date: Thu, 22 Jun 2017 14:21:23 -0700
Subject: [PATCH 02/10] Fix compile error.

---
 Swarmathon_Arduino/Swarmathon_Arduino.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Swarmathon_Arduino/Swarmathon_Arduino.ino b/Swarmathon_Arduino/Swarmathon_Arduino.ino
index 8ec1fa0..7975a5f 100644
--- a/Swarmathon_Arduino/Swarmathon_Arduino.ino
+++ b/Swarmathon_Arduino/Swarmathon_Arduino.ino
@@ -278,7 +278,7 @@ String updateOdom() {
 
   txBuffer = String(odom.left) + "," +
              String(odom.right) + "," +
-             String(odom.clock)
+             String(odom.clock);
 
   return txBuffer;
 }

From 42938ba54ff31912943590f2d2f08fed7a258cfd Mon Sep 17 00:00:00 2001
From: Mike Matera <mike-matera@users.noreply.github.com>
Date: Thu, 22 Jun 2017 14:23:21 -0700
Subject: [PATCH 03/10] Update Odometry.cpp

---
 libraries/Odometry/Odometry.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/libraries/Odometry/Odometry.cpp b/libraries/Odometry/Odometry.cpp
index 4f9cdd9..b66f626 100644
--- a/libraries/Odometry/Odometry.cpp
+++ b/libraries/Odometry/Odometry.cpp
@@ -35,7 +35,6 @@ Odometry::Odometry(byte rightEncoderAPin, byte rightEncoderBPin, byte leftEncode
     _leftEncoderBPin = leftEncoderBPin;
     right = 0;
     left = 0;
-    ts = 0;
     clock = millis();
 }
 

From 882a1cbc7a6a3f193f9e9ae3cc768b21b6651126 Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Thu, 3 Aug 2017 19:19:36 -0700
Subject: [PATCH 04/10] Fix signed math in Odometry

---
 libraries/Odometry/Odometry.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/Odometry/Odometry.h b/libraries/Odometry/Odometry.h
index 885a1ac..5fc6ba1 100644
--- a/libraries/Odometry/Odometry.h
+++ b/libraries/Odometry/Odometry.h
@@ -12,7 +12,7 @@ class Odometry {
     void update();
     
     //Variables
-    uint16_t right, left;
+    int16_t right, left;
     long clock;
 };
 

From db2a5c03c1725468629698ad934606ac2904835c Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Wed, 13 Sep 2017 09:58:52 -0700
Subject: [PATCH 05/10] Added CRC32 library needed for safe EEPROM use

---
 libraries/CRC32/README.md                |   4 +
 libraries/CRC32/examples/CRC32/CRC32.ino |  70 ++++++++++++++
 libraries/CRC32/keywords.txt             |  20 ++++
 libraries/CRC32/library.properties       |   9 ++
 libraries/CRC32/src/CRC32.cpp            | 115 +++++++++++++++++++++++
 libraries/CRC32/src/CRC32.h              |  66 +++++++++++++
 6 files changed, 284 insertions(+)
 create mode 100644 libraries/CRC32/README.md
 create mode 100644 libraries/CRC32/examples/CRC32/CRC32.ino
 create mode 100644 libraries/CRC32/keywords.txt
 create mode 100644 libraries/CRC32/library.properties
 create mode 100644 libraries/CRC32/src/CRC32.cpp
 create mode 100644 libraries/CRC32/src/CRC32.h

diff --git a/libraries/CRC32/README.md b/libraries/CRC32/README.md
new file mode 100644
index 0000000..63e5895
--- /dev/null
+++ b/libraries/CRC32/README.md
@@ -0,0 +1,4 @@
+CRC32
+=====
+
+An Arduino library for calculating a CRC32 checksum.
diff --git a/libraries/CRC32/examples/CRC32/CRC32.ino b/libraries/CRC32/examples/CRC32/CRC32.ino
new file mode 100644
index 0000000..316e8f1
--- /dev/null
+++ b/libraries/CRC32/examples/CRC32/CRC32.ino
@@ -0,0 +1,70 @@
+// =============================================================================
+//
+// Copyright (c) 2013-2016 Christopher Baker <http://christopherbaker.net>
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// =============================================================================
+
+#include <CRC32.h>
+
+void setup()
+{
+  // Begin serial output for testing / debugging.
+  Serial.begin(115200);
+}
+
+void loop()
+{
+  // The known CRC32 Checksum for the "Hello World" string below.
+  const uint32_t KNOWN_CHECKSUM = 0x4A17B156;
+
+  // Create some test data. This is an array of arbitrary bytes.
+  // For this test, we'll create an array of bytes representing text.
+  uint8_t byteBuffer[] = "Hello World";
+  size_t numBytes = sizeof(byteBuffer) - 1;
+
+  // Create a CRC32 checksum calculator.
+  CRC32 crc;
+
+  // Here we add each byte to the checksum, caclulating the checksum as we go.
+  for (size_t i = 0; i < numBytes; i++)
+  {
+    crc.update(byteBuffer[i]);
+  }
+
+  // Alternatively, we can add an array of bytes in bulk.
+  // crc.update(byteBuffer, numBytes);
+
+  // Once we have added all of the data, generate the final CRC32 checksum.
+  uint32_t checksum = crc.finalize();
+
+  if (checksum == KNOWN_CHECKSUM)
+  {
+    Serial.println(F("TEST PASSED"));
+  }
+  else
+  {
+    Serial.println(F("TEST FAILED"));
+  }
+
+  // Wait a little bit because this is just a test.
+  delay(3000);
+
+}
diff --git a/libraries/CRC32/keywords.txt b/libraries/CRC32/keywords.txt
new file mode 100644
index 0000000..7cdfb20
--- /dev/null
+++ b/libraries/CRC32/keywords.txt
@@ -0,0 +1,20 @@
+#######################################
+# Syntax Coloring Map For Arduration
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+CRC32	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+calculate	KEYWORD2
+update	KEYWORD2
+finalize	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
diff --git a/libraries/CRC32/library.properties b/libraries/CRC32/library.properties
new file mode 100644
index 0000000..b50e972
--- /dev/null
+++ b/libraries/CRC32/library.properties
@@ -0,0 +1,9 @@
+name=CRC32
+version=1.1.0
+author=Christopher Baker <info@christopherbaker.net>
+maintainer=Christopher Baker <info@christopherbaker.net>
+sentence=An Arduino library for calculating a CRC32 checksum.
+paragraph=An Arduino library for calculating a CRC32 checksum.
+category=Data Processing
+url=https://github.com/bakercp/CRC32
+architectures=*
diff --git a/libraries/CRC32/src/CRC32.cpp b/libraries/CRC32/src/CRC32.cpp
new file mode 100644
index 0000000..4b2523f
--- /dev/null
+++ b/libraries/CRC32/src/CRC32.cpp
@@ -0,0 +1,115 @@
+// =============================================================================
+//
+// Copyright (c) 2013-2016 Christopher Baker <http://christopherbaker.net>
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// =============================================================================
+
+
+#include "CRC32.h"
+
+#if defined(PROGMEM)
+  #define FLASH_PROGMEM PROGMEM
+  #define FLASH_READ_DWORD(x) (pgm_read_dword_near(x))
+#else
+  #define FLASH_PROGMEM
+  #define FLASH_READ_DWORD(x) (*(uint32_t*)(x))
+#endif
+
+static const uint32_t crc32_table[] FLASH_PROGMEM = {
+  0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac,
+  0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c,
+  0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c,
+  0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c
+};
+
+uint32_t CRC32::calculate(const void* data, size_t size)
+{
+  CRC32 crc;
+  crc.update(data, size);
+  return crc.finalize();
+}
+
+void CRC32::reset()
+{
+  state = ~0L;
+}
+
+void CRC32::update(uint8_t data)
+{
+  // via http://forum.arduino.cc/index.php?topic=91179.0
+  uint8_t tbl_idx;
+
+  tbl_idx = state ^ (data >> (0 * 4));
+  state = FLASH_READ_DWORD(crc32_table + (tbl_idx & 0x0f)) ^ (state >> 4);
+  tbl_idx = state ^ (data >> (1 * 4));
+  state = FLASH_READ_DWORD(crc32_table + (tbl_idx & 0x0f)) ^ (state >> 4);
+}
+
+void CRC32::update(const void* data, size_t size)
+{
+  const uint8_t* d = (const uint8_t*)data;
+  while (size--) {
+    update(*d++);
+  }
+}
+
+uint32_t CRC32::finalize(const void* data, size_t size)
+{
+  update(data, size);
+  return finalize();
+}
+
+uint32_t CRC32::finalize() const
+{
+  return ~state;
+}
+
+// Deprecated API
+
+uint32_t CRC32::checksum(const uint8_t* data, size_t size)
+{
+    CRC32 crc;
+    crc.update(data, size);
+    return crc.finalize();
+}
+
+
+uint32_t CRC32::update(uint32_t checksum, uint8_t data)
+{
+    // via http://forum.arduino.cc/index.php?topic=91179.0
+    uint8_t tbl_idx;
+
+    tbl_idx = checksum ^ (data >> (0 * 4));
+    checksum = pgm_read_dword_near(crc32_table + (tbl_idx & 0x0f)) ^ (checksum >> 4);
+    tbl_idx = checksum ^ (data >> (1 * 4));
+    checksum = pgm_read_dword_near(crc32_table + (tbl_idx & 0x0f)) ^ (checksum >> 4);
+
+    return checksum;
+}
+
+
+uint32_t CRC32::update(uint32_t checksum, const uint8_t* data, size_t size)
+{
+    while(size--) checksum = update(checksum, *data++);
+
+    return checksum;
+}
+
diff --git a/libraries/CRC32/src/CRC32.h b/libraries/CRC32/src/CRC32.h
new file mode 100644
index 0000000..47ae2ee
--- /dev/null
+++ b/libraries/CRC32/src/CRC32.h
@@ -0,0 +1,66 @@
+// =============================================================================
+//
+// Copyright (c) 2013-2016 Christopher Baker <http://christopherbaker.net>
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// =============================================================================
+
+#ifndef CRC32_H
+#define CRC32_H
+
+#if defined(SPARK) || defined(PARTICLE)
+    #include "application.h"
+#elif defined(ARDUINO)
+    #if ARDUINO >= 100
+        #include "Arduino.h"
+    #else
+        #include "WProgram.h"
+    #endif
+#endif
+
+class CRC32
+{
+public:
+  static uint32_t calculate(const void* data, size_t size);
+
+public:
+  CRC32() { reset(); }
+
+  void reset();
+  void update(uint8_t data);
+  void update(const void* data, size_t size);
+  uint32_t finalize(const void* data, size_t size);
+  uint32_t finalize() const;
+
+  // Deprecated API
+  __attribute__ ((deprecated))
+  static uint32_t checksum(const uint8_t* data, size_t size);
+
+  __attribute__ ((deprecated))
+  static uint32_t update(uint32_t checksum, uint8_t data);
+
+  __attribute__ ((deprecated))
+  static uint32_t update(uint32_t checksum, const uint8_t* data, size_t size);
+
+private:
+  uint32_t state;
+};
+
+#endif

From e463cc7a6e9c79e440bc0ce7b1e17c10d4f4b781 Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Wed, 13 Sep 2017 09:59:31 -0700
Subject: [PATCH 06/10] Update Arduino to optionally use EEPROM stored
 calibration.

---
 Swarmathon_Arduino/Swarmathon_Arduino.ino | 93 ++++++++++++++++++++++-
 1 file changed, 90 insertions(+), 3 deletions(-)

diff --git a/Swarmathon_Arduino/Swarmathon_Arduino.ino b/Swarmathon_Arduino/Swarmathon_Arduino.ino
index 7975a5f..2fd3f86 100644
--- a/Swarmathon_Arduino/Swarmathon_Arduino.ino
+++ b/Swarmathon_Arduino/Swarmathon_Arduino.ino
@@ -13,6 +13,8 @@
 #include <NewPing.h>
 #include <Odometry.h>
 #include <Servo.h>
+#include <EEPROM.h>
+#include <CRC32.h>
 
 // Constants
 #define PI 3.14159265358979323846
@@ -74,6 +76,30 @@ NewPing leftUS(leftSignal, leftSignal, 330);
 NewPing centerUS(centerSignal, centerSignal, 330);
 NewPing rightUS(rightSignal, rightSignal, 330);
 
+////////////////////////////
+////Calibration         ////
+////////////////////////////
+
+#define CAL_HEADER_WORD 0xaa995566
+
+// Track the maximum/minimum magnetometer values for runtime calibration.
+// These will be invalid as soon as the rover drives because the field
+// generated by the motors. These will be stored when abridge sends a command.
+struct cal_t {
+  uint32_t header = CAL_HEADER_WORD;
+  struct cal_data_t { 
+    uint16_t x_min = 65535;
+    uint16_t x_max = 0;
+    uint16_t y_min = 65535;
+    uint16_t y_max = 0;
+    uint16_t z_min = 65535;
+    uint16_t z_max = 0;
+  } data;
+  uint32_t crc32 = 0;
+} calibration;
+
+void commitCalibration();
+bool readCalibration(cal_t &cal);
 
 /////////////
 ////Setup////
@@ -220,14 +246,49 @@ void parse() {
     angle = wristMin + (wristMax/370) * angle;
     wrist.writeMicroseconds(angle);
   }
+  else if (rxBuffer == "C") {
+    // Commit the current observed calibration to EEPROM.
+    commitCalibration();
+  }
 }
 
+void commitCalibration() {
+  CRC32 gen;
+
+  // Generate the crc32
+  calibration.crc32 = gen.calculate(&calibration.data, sizeof(cal_t::cal_data_t));
+
+  // Write EERPOM data
+  for (int i=0; i<sizeof(cal_t); i++) {
+    EEPROM.write(i, ((uint8_t *) &calibration)[i]);
+  }
+}
+
+bool readCalibration(cal_t &cal) {
+  CRC32 gen;
+
+  // Read the EEPROM data
+  for (int i=0; i<sizeof(cal_t); i++) {
+    ((uint8_t *) &cal)[i] = EEPROM.read(i); 
+  }
+
+  // Validate the header
+  if (cal.header != CAL_HEADER_WORD)
+    return false;
+
+  // Validate the signature
+  uint32_t expected_sig = gen.calculate(&cal.data, sizeof(cal_t::cal_data_t));
+  if (cal.crc32 != expected_sig)
+    return false;
+    
+  return true;
+}
 
 //////////////////////////
 //Update transmit buffer//
 //////////////////////////
 
-String updateIMU() {
+String updateIMU() {  
   //Update current sensor values
   gyroscope.read();
   magnetometer_accelerometer.read();
@@ -238,6 +299,22 @@ String updateIMU() {
     L3G::vector<int16_t> gyro = gyroscope.g;
     LSM303::vector<int16_t> mag = magnetometer_accelerometer.m;
 
+    // Update runtime calibration
+    if (calibration.data.x_min > mag.x)
+      calibration.data.x_min = mag.x;
+    if (calibration.data.x_max < mag.x)
+      calibration.data.x_max = mag.x;
+
+    if (calibration.data.y_min > mag.y)
+      calibration.data.y_min = mag.y;
+    if (calibration.data.y_max < mag.y)
+      calibration.data.y_max = mag.y;
+    
+    if (calibration.data.z_min > mag.z)
+      calibration.data.z_min = mag.z;
+    if (calibration.data.z_max < mag.z)
+      calibration.data.z_max = mag.z;
+	
     //Convert accelerometer digits to milligravities, then to gravities, and finally to meters per second squared
     LSM303::vector<float> linear_acceleration = {acc.y*0.061/1000*9.81, -acc.x*0.061/1000*9.81, acc.z*0.061/1000*9.81};
 
@@ -296,8 +373,18 @@ void imuInit() {
 
   magnetometer_accelerometer.init();
   magnetometer_accelerometer.enableDefault();
-  magnetometer_accelerometer.m_min = (LSM303::vector<int16_t>){ -2247,  -2068,  -1114};
-  magnetometer_accelerometer.m_max = (LSM303::vector<int16_t>){+3369,  +2877,  +3634};
+
+  cal_t stored_cal;
+  if (readCalibration(stored_cal)) {
+    magnetometer_accelerometer.m_min = (LSM303::vector<int16_t>)
+      { stored_cal.data.x_min, stored_cal.data.y_min, stored_cal.data.z_min };
+    magnetometer_accelerometer.m_max = (LSM303::vector<int16_t>)
+      { stored_cal.data.x_max, stored_cal.data.y_max, stored_cal.data.z_max };
+  }else{
+    magnetometer_accelerometer.m_min = (LSM303::vector<int16_t>){ -2247,  -2068,  -1114};
+    magnetometer_accelerometer.m_max = (LSM303::vector<int16_t>){+3369,  +2877,  +3634};
+  }
+  
   magnetometer_accelerometer.setTimeout(1);
 
   pressure.init();

From cb909925831f8822a76c3ae7a858037554ab644b Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Wed, 13 Sep 2017 09:59:51 -0700
Subject: [PATCH 07/10] Ignore Arduino build artifacts.

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)
 create mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..8e38ac0
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+
+build

From 7d1bfe34a15c21a749835fd8769a268b8b8a594c Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Wed, 13 Sep 2017 11:56:08 -0700
Subject: [PATCH 08/10] Fix integer type in Odom to match the return value of
 millis()

---
 libraries/Odometry/Odometry.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/Odometry/Odometry.h b/libraries/Odometry/Odometry.h
index 5fc6ba1..66e0c0e 100644
--- a/libraries/Odometry/Odometry.h
+++ b/libraries/Odometry/Odometry.h
@@ -13,7 +13,7 @@ class Odometry {
     
     //Variables
     int16_t right, left;
-    long clock;
+    unsigned long clock;
 };
 
 #endif

From b8664c789a7f81099fcb5cee12b9fdbdbd83c87c Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Wed, 13 Sep 2017 11:56:50 -0700
Subject: [PATCH 09/10] Fix types/limits of calibration constants. Add
 debugging code.

---
 Swarmathon_Arduino/Swarmathon_Arduino.ino | 66 ++++++++++++++++++++---
 1 file changed, 60 insertions(+), 6 deletions(-)

diff --git a/Swarmathon_Arduino/Swarmathon_Arduino.ino b/Swarmathon_Arduino/Swarmathon_Arduino.ino
index 2fd3f86..66cbdf4 100644
--- a/Swarmathon_Arduino/Swarmathon_Arduino.ino
+++ b/Swarmathon_Arduino/Swarmathon_Arduino.ino
@@ -88,17 +88,19 @@ NewPing rightUS(rightSignal, rightSignal, 330);
 struct cal_t {
   uint32_t header = CAL_HEADER_WORD;
   struct cal_data_t { 
-    uint16_t x_min = 65535;
-    uint16_t x_max = 0;
-    uint16_t y_min = 65535;
-    uint16_t y_max = 0;
-    uint16_t z_min = 65535;
-    uint16_t z_max = 0;
+    int16_t x_min = INT16_MAX;
+    int16_t x_max = INT16_MIN;
+    int16_t y_min = INT16_MAX;
+    int16_t y_max = INT16_MIN;
+    int16_t z_min = INT16_MAX;
+    int16_t z_max = INT16_MIN;
   } data;
   uint32_t crc32 = 0;
 } calibration;
+bool stored_calibration_used = false;
 
 void commitCalibration();
+void clearCalibration();
 bool readCalibration(cal_t &cal);
 
 /////////////
@@ -250,6 +252,57 @@ void parse() {
     // Commit the current observed calibration to EEPROM.
     commitCalibration();
   }
+  else if (rxBuffer == "X") {
+    // Clear the EEPROM calibration.
+    clearCalibration();
+  }
+  else if (rxBuffer == "P") {
+    // Print the current measured and stored calibration for debugging.
+    Serial.println("Runtime calibration:");
+    Serial.print("  x (min/max): ");
+    Serial.print(calibration.data.x_min);
+    Serial.print(" / ");
+    Serial.println(calibration.data.x_max);
+    
+    Serial.print("  y (min/max): ");
+    Serial.print(calibration.data.y_min);
+    Serial.print(" / ");
+    Serial.println(calibration.data.y_max);
+
+    Serial.print("  z (min/max): ");
+    Serial.print(calibration.data.z_min);
+    Serial.print(" / ");
+    Serial.println(calibration.data.z_max);
+
+    Serial.println();
+    Serial.println("Curerent calibration:");
+    Serial.print("  x (min/max): ");
+    Serial.print(magnetometer_accelerometer.m_min.x);
+    Serial.print(" / ");
+    Serial.println(magnetometer_accelerometer.m_max.x);
+    
+    Serial.print("  y (min/max): ");
+    Serial.print(magnetometer_accelerometer.m_min.y);
+    Serial.print(" / ");
+    Serial.println(magnetometer_accelerometer.m_max.y);
+
+    Serial.print("  z (min/max): ");
+    Serial.print(magnetometer_accelerometer.m_min.z);
+    Serial.print(" / ");
+    Serial.println(magnetometer_accelerometer.m_max.z);
+
+    Serial.println();
+    if (stored_calibration_used)
+      Serial.println("Stored calibration was used.");
+    else
+      Serial.println("Stored calibration was NOT used.");      
+  }
+}
+
+void clearCalibration() {
+  for (int i=0; i<sizeof(cal_t); i++) {
+    EEPROM.write(i, 0xff);
+  }
 }
 
 void commitCalibration() {
@@ -376,6 +429,7 @@ void imuInit() {
 
   cal_t stored_cal;
   if (readCalibration(stored_cal)) {
+    stored_calibration_used = true;
     magnetometer_accelerometer.m_min = (LSM303::vector<int16_t>)
       { stored_cal.data.x_min, stored_cal.data.y_min, stored_cal.data.z_min };
     magnetometer_accelerometer.m_max = (LSM303::vector<int16_t>)

From 7a2a3c9aa3424c1f39061bb563940f08bca7f9d2 Mon Sep 17 00:00:00 2001
From: Mike Matera <matera@lifealgorithmic.com>
Date: Sat, 16 Sep 2017 12:22:04 -0700
Subject: [PATCH 10/10] Command added to reset measured calibration.

---
 Swarmathon_Arduino/Swarmathon_Arduino.ino | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/Swarmathon_Arduino/Swarmathon_Arduino.ino b/Swarmathon_Arduino/Swarmathon_Arduino.ino
index 66cbdf4..96663c5 100644
--- a/Swarmathon_Arduino/Swarmathon_Arduino.ino
+++ b/Swarmathon_Arduino/Swarmathon_Arduino.ino
@@ -101,6 +101,7 @@ bool stored_calibration_used = false;
 
 void commitCalibration();
 void clearCalibration();
+void clearObservedCalibration();
 bool readCalibration(cal_t &cal);
 
 /////////////
@@ -256,6 +257,10 @@ void parse() {
     // Clear the EEPROM calibration.
     clearCalibration();
   }
+  else if (rxBuffer == "M") {
+    // Clear the in-memory min/max.
+    clearObservedCalibration();
+  }
   else if (rxBuffer == "P") {
     // Print the current measured and stored calibration for debugging.
     Serial.println("Runtime calibration:");
@@ -305,6 +310,15 @@ void clearCalibration() {
   }
 }
 
+void clearObservedCalibration() {
+  calibration.data.x_min = INT16_MAX;
+  calibration.data.x_max = INT16_MIN;
+  calibration.data.y_min = INT16_MAX;
+  calibration.data.y_max = INT16_MIN;
+  calibration.data.z_min = INT16_MAX;
+  calibration.data.z_max = INT16_MIN;
+}
+
 void commitCalibration() {
   CRC32 gen;