From 0a40d27bea5a27a29cf992bc88047bae3e38b34c Mon Sep 17 00:00:00 2001
From: jcchurch13 <jcchurch@mit.edu>
Date: Wed, 31 May 2017 00:52:02 -0400
Subject: [PATCH] Fixed cal bug

---
 Mechaduino/Mechaduino/Utils.cpp | 22 ++++++++++++++++++++--
 1 file changed, 20 insertions(+), 2 deletions(-)

diff --git a/Mechaduino/Mechaduino/Utils.cpp b/Mechaduino/Mechaduino/Utils.cpp
index 5944a12..2bdadb2 100644
--- a/Mechaduino/Mechaduino/Utils.cpp
+++ b/Mechaduino/Mechaduino/Utils.cpp
@@ -141,6 +141,7 @@ void output(float theta, int effort) {
 void calibrate() {   /// this is the calibration routine
 
   int encoderReading = 0;     //or float?  not sure if we can average for more res?
+  int currentencoderReading = 0;
   int lastencoderReading = 0;
   int avg = 10;               //how many readings to average
 
@@ -182,12 +183,29 @@ void calibrate() {   /// this is the calibration routine
 
     encoderReading = 0;
     delay(100);                         //moving too fast may not give accurate readings.  Motor needs time to settle after each step.
-    
+    lastencoderReading = readEncoder();
+        
     for (int reading = 0; reading < avg; reading++) {  //average multple readings at each step
-      encoderReading += readEncoder();
+      currentencoderReading = readEncoder();
+
+      if ((currentencoderReading-lastencoderReading)<(-(cpr/2))){
+        currentencoderReading += cpr;
+      }
+      else if ((currentencoderReading-lastencoderReading)>((cpr/2))){
+        currentencoderReading -= cpr;
+      }
+ 
+      encoderReading += currentencoderReading;
       delay(10);
+      lastencoderReading = currentencoderReading;
     }
     encoderReading = encoderReading / avg;
+    if (encoderReading>cpr){
+      encoderReading-= cpr;
+    }
+    else if (encoderReading<0){
+      encoderReading+= cpr;
+    }
 
     fullStepReadings[x] = encoderReading;
    // SerialUSB.println(fullStepReadings[x], DEC);      //print readings as a sanity check