Arduino reports encoder ticks directly.

.gitignore

@@ -0,0 +1,2 @@
+
+build

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
@@ -67,13 +69,40 @@ 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);
 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 { 
+    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();
+void clearObservedCalibration();
+bool readCalibration(cal_t &cal);
 
 /////////////
 ////Setup////
@@ -220,14 +249,113 @@ void parse() {
     angle = wristMin + (wristMax/370) * angle;
     wrist.writeMicroseconds(angle);
   }
+  else if (rxBuffer == "C") {
+    // Commit the current observed calibration to EEPROM.
+    commitCalibration();
+  }
+  else if (rxBuffer == "X") {
+    // 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:");
+    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 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;
+
+  // 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 +366,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};
 
@@ -276,12 +420,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;
 }
@@ -299,8 +440,19 @@ 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)) {
+    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>)
+      { 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();

libraries/CRC32/README.md

@@ -0,0 +1,4 @@
+CRC32
+=====
+
+An Arduino library for calculating a CRC32 checksum.

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);
+
+}

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)
+#######################################
+

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=*