From 0e588212535f3d96736c9b284f1b7f0e78e72a15 Mon Sep 17 00:00:00 2001
From: whothat <benhuff322@gmail.com>
Date: Mon, 13 Jun 2016 20:30:22 -0700
Subject: [PATCH 1/7] Add Recorder class

---
 MicroMouse-2016.vcxproj         |  1 +
 MicroMouse-2016.vcxproj.filters |  3 +++
 micromouse/RobotIO.cpp          | 35 +++++++++++++++++++++++++++------
 micromouse/RobotIO.h            |  2 ++
 4 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/MicroMouse-2016.vcxproj b/MicroMouse-2016.vcxproj
index a784729..4570998 100644
--- a/MicroMouse-2016.vcxproj
+++ b/MicroMouse-2016.vcxproj
@@ -177,6 +177,7 @@
     <ClInclude Include="micromouse\Node.h" />
     <ClInclude Include="micromouse\Path.h" />
     <ClInclude Include="micromouse\PIDController.h" />
+    <ClInclude Include="micromouse\Recorder.h" />
     <ClInclude Include="micromouse\RobotIO.h" />
     <ClInclude Include="micromouse\IRSensor.h" />
     <ClInclude Include="micromouse\Timer.h" />
diff --git a/MicroMouse-2016.vcxproj.filters b/MicroMouse-2016.vcxproj.filters
index e0fe22e..60ee33f 100644
--- a/MicroMouse-2016.vcxproj.filters
+++ b/MicroMouse-2016.vcxproj.filters
@@ -110,5 +110,8 @@
     <ClInclude Include="micromouse\ButtonFlag.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="micromouse\Recorder.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/micromouse/RobotIO.cpp b/micromouse/RobotIO.cpp
index 91bda74..5d344ab 100644
--- a/micromouse/RobotIO.cpp
+++ b/micromouse/RobotIO.cpp
@@ -4,6 +4,7 @@
 #include "Logger.h"
 #include "Timer.h"
 #include "ButtonFlag.h"
+#include "Recorder.h"
 
 
 
@@ -168,12 +169,7 @@ namespace Micromouse
 		//rotate(90.0f);
 		//delay(2000);
 		moveForward(180.0f);
-		moveForward(180.0f);
-		moveForward(180.0f);
-		moveForward(180.0f);
-		moveForward(180.0f);
-		moveForward(180.0f);
-		moveForward(180.0f);
+
 /*
 		rightMotor.setMaxSpeed(0.2f);
 		leftMotor.setMaxSpeed(0.2f);
@@ -245,6 +241,9 @@ namespace Micromouse
 		//millimeters represents how much farther the bot needs to travel.
 		//The function will loop until centimeters is within DISTANCE_TOLERANCE
 
+		Recorder<float> rec(6000);
+		Recorder<float> rec2(6000);
+
 		float leftmm = millimeters;
 		float rightmm = millimeters;
 
@@ -296,6 +295,8 @@ namespace Micromouse
 			//Get distance traveled in cm since last cycle (average of two encoders)
 			float leftTraveled = leftMotor.resetCounts();
 			float rightTraveled = rightMotor.resetCounts();
+
+
 			leftTraveled /= COUNTS_PER_MM;
 			rightTraveled /= COUNTS_PER_MM;
 
@@ -306,9 +307,24 @@ namespace Micromouse
 			leftmm -= leftTraveled;
 			rightmm -= rightTraveled;
 
+
+
+	
+
+
 			leftSpeed = leftDistPID.getCorrection(leftmm);
 			rightSpeed = rightDistPID.getCorrection(rightmm);
 
+			if (!rec.addValue(leftSpeed))
+			{
+				stopMotors();
+				rec.print();
+				
+				rec2.print();
+			}
+			rec2.addValue(leftmm);
+
+
 			float speedError = actualLeftSpeed - actualRightSpeed;
 			float speedCorrection = speedPID.getCorrection(speedError);
 			//speedCorrection *= -1;
@@ -559,4 +575,11 @@ namespace Micromouse
 		IRSensors[FRONT_LEFT]->saveCalibration(IR_FRONT_LEFT_MEMORY);
 		IRSensors[FRONT_RIGHT]->saveCalibration(IR_FRONT_RIGHT_MEMORY);
 	}
+
+
+	void RobotIO::stopMotors()
+	{
+		leftMotor.brake();
+		rightMotor.brake();
+	}
 }
diff --git a/micromouse/RobotIO.h b/micromouse/RobotIO.h
index bca4563..2a579b6 100644
--- a/micromouse/RobotIO.h
+++ b/micromouse/RobotIO.h
@@ -88,6 +88,8 @@ namespace Micromouse
 
 		void calibrateIRSensors();
 
+		void stopMotors();
+
 	private:
 		enum IRDirection { LEFT, RIGHT, FRONT_LEFT, FRONT_RIGHT };
 

From 3001c9f7fcb1f04cc6420663461c3f8d74939a6f Mon Sep 17 00:00:00 2001
From: whothat <benhuff322@gmail.com>
Date: Tue, 14 Jun 2016 08:06:03 -0700
Subject: [PATCH 2/7] add multiple variable tracking

---
 micromouse/Recorder.h  | 96 ++++++++++++++++++++++++++++++++++++++++++
 micromouse/RobotIO.cpp | 13 +++---
 2 files changed, 103 insertions(+), 6 deletions(-)
 create mode 100644 micromouse/Recorder.h

diff --git a/micromouse/Recorder.h b/micromouse/Recorder.h
new file mode 100644
index 0000000..7ac19b8
--- /dev/null
+++ b/micromouse/Recorder.h
@@ -0,0 +1,96 @@
+#pragma once
+#include "Logger.h"
+
+namespace Micromouse {
+
+
+	template<typename T>
+	class Recorder
+	{
+	public:
+		Recorder(int size = 1000, int dimension = 1);
+		~Recorder();
+
+		bool addValue(T val, int dimension);
+		void print();
+
+		const int SIZE, DIMENSION;
+
+		int* ind;
+
+		T** arrs;
+
+
+
+
+
+	};
+
+
+	template<typename T>
+	Recorder<T>::Recorder(int size, int dimension) :
+		SIZE(size),
+		DIMENSION(dimension)
+	{
+		ind = new int [DIMENSION];
+		arrs = new T* [DIMENSION];
+		
+		for (size_t i = 0; i < DIMENSION; i++)
+		{
+			arrs[i] = new T[SIZE];
+			ind[i] = 0;
+		}
+	}
+
+
+	template<typename T>
+	Recorder<T>::~Recorder()
+	{
+		for (size_t i = 0; i < DIMENSION; i++)
+		{
+			delete[] arrs[i];
+		}
+
+		delete[] arrs;
+		delete[] ind;
+	}
+
+
+	template<typename T>
+	bool Recorder<T>::addValue(T val, int dimension)
+	{
+		if (ind[dimension] < SIZE)
+		{
+			arrs[dimension][ind[dimension]++] = val;
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+
+	template<typename T>
+	void Recorder<T>::print()
+	{
+		buttonFlag = false;
+#ifdef __MK20DX256__ // Teensy Compile
+		while (!buttonFlag)
+		{
+			delay(10);
+		}
+
+		for (int i = 0; i < ind[0]; i++)
+		{
+			for (int j = 0; j < DIMENSION - 1; j++)
+			{
+				Serial.print(arrs[j][i]);
+				Serial.print(", ");
+			}
+			Serial.println(arrs[DIMENSION-1][i]);
+			delay(1);
+		}
+#endif
+		buttonFlag = false;
+	}
+}
\ No newline at end of file
diff --git a/micromouse/RobotIO.cpp b/micromouse/RobotIO.cpp
index 5d344ab..cb2076f 100644
--- a/micromouse/RobotIO.cpp
+++ b/micromouse/RobotIO.cpp
@@ -241,8 +241,7 @@ namespace Micromouse
 		//millimeters represents how much farther the bot needs to travel.
 		//The function will loop until centimeters is within DISTANCE_TOLERANCE
 
-		Recorder<float> rec(6000);
-		Recorder<float> rec2(6000);
+		Recorder<float> rec(6000,2);
 
 		float leftmm = millimeters;
 		float rightmm = millimeters;
@@ -314,15 +313,15 @@ namespace Micromouse
 
 			leftSpeed = leftDistPID.getCorrection(leftmm);
 			rightSpeed = rightDistPID.getCorrection(rightmm);
+			
+			rec.addValue(leftmm,1);
 
-			if (!rec.addValue(leftSpeed))
+			if (!rec.addValue(leftSpeed,0))
 			{
 				stopMotors();
 				rec.print();
-				
-				rec2.print();
 			}
-			rec2.addValue(leftmm);
+			
 
 
 			float speedError = actualLeftSpeed - actualRightSpeed;
@@ -380,6 +379,8 @@ namespace Micromouse
 		leftMotor.brake();
 		rightMotor.brake();
 
+		rec.print();
+
 	}
 
 

From d25f1970b16cbe62a63ad4564e8ef7005f034fa2 Mon Sep 17 00:00:00 2001
From: joshuasrjc <joshuasrjc@gmail.com>
Date: Sun, 17 Jul 2016 17:18:43 -0700
Subject: [PATCH 3/7] Fixed PID Bug

---
 micromouse/PIDController.cpp | 12 ++++++++++--
 micromouse/PIDController.h   |  4 ++++
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/micromouse/PIDController.cpp b/micromouse/PIDController.cpp
index d2232ee..1de7a50 100644
--- a/micromouse/PIDController.cpp
+++ b/micromouse/PIDController.cpp
@@ -49,8 +49,17 @@ namespace Micromouse
 		float iCorrection = I * totalError;
 		float dCorrection = D * (currentError - lastError);
 
+		lastError = currentError; 
+
+		pCorrection /= 1000;
+		iCorrection /= 1000;
+		dCorrection /= 1000;
+
+		lastP_Correction = pCorrection;
+		lastI_Correction = iCorrection;
+		lastD_Correction = dCorrection;
+
 		float sum = pCorrection + iCorrection + dCorrection;
-		sum /= 1000.0f;
 
 		//Sum is bounded between -1 and 1
 		sum = sum < -1 ? -1 : sum;
@@ -67,7 +76,6 @@ namespace Micromouse
 	}
 
 
-
 	void PIDController::setConstants(float P, float I, float D)
 	{
 		this->P = P;
diff --git a/micromouse/PIDController.h b/micromouse/PIDController.h
index 5ad54cc..6f2a01a 100644
--- a/micromouse/PIDController.h
+++ b/micromouse/PIDController.h
@@ -37,6 +37,10 @@ namespace Micromouse
 
 		float getI() const;
 
+		float lastP_Correction = 0.0f;
+		float lastI_Correction = 0.0f;
+		float lastD_Correction = 0.0f;
+
 		// Sets the P, I, and D constants for the controller.
 		void setConstants(float P, float I, float D);
 

From d1aaebe51ff0a771f645fc9d3f8d293df6add62e Mon Sep 17 00:00:00 2001
From: joshuasrjc <joshuasrjc@gmail.com>
Date: Sun, 17 Jul 2016 17:20:28 -0700
Subject: [PATCH 4/7] Added Data Logging for PID

---
 micromouse/RobotIO.cpp | 36 ++++++++++++++++++++++++------------
 1 file changed, 24 insertions(+), 12 deletions(-)

diff --git a/micromouse/RobotIO.cpp b/micromouse/RobotIO.cpp
index cb2076f..d1ec52f 100644
--- a/micromouse/RobotIO.cpp
+++ b/micromouse/RobotIO.cpp
@@ -168,7 +168,7 @@ namespace Micromouse
 #ifdef __MK20DX256__ // Teensy Compile
 		//rotate(90.0f);
 		//delay(2000);
-		moveForward(180.0f);
+		moveForward(6*180.0f);
 
 /*
 		rightMotor.setMaxSpeed(0.2f);
@@ -235,19 +235,19 @@ namespace Micromouse
 	}
 
 
-
+	
 	void RobotIO::moveForward(float millimeters)
 	{
 		//millimeters represents how much farther the bot needs to travel.
 		//The function will loop until centimeters is within DISTANCE_TOLERANCE
 
-		Recorder<float> rec(6000,2);
+		Recorder<float> rec(3000,4);
 
 		float leftmm = millimeters;
 		float rightmm = millimeters;
 
-		PIDController leftDistPID = PIDController(97.0f, 46.0f, 16.0f , 1000);
-		PIDController rightDistPID = PIDController(97.0f, 46.0f, 16.0f , 1000);
+		PIDController leftDistPID = PIDController(97.0f, 46.0f, 160.0f , 1000);
+		PIDController rightDistPID = PIDController(97.0f, 46.0f, 160.0f , 1000);
 
 		PIDController speedPID = PIDController(30.0f, 1.0f, 1.0f);
 
@@ -286,6 +286,7 @@ namespace Micromouse
 			BUTTONFLAG
 
 			float deltaTime = timer.getDeltaTime();
+			
 
 			//Get distance from the front of the bot to the wall.
 			frontRightIRDist = IRSensors[FRONT_RIGHT]->getDistance();
@@ -307,21 +308,27 @@ namespace Micromouse
 			rightmm -= rightTraveled;
 
 
-
-	
-
-
 			leftSpeed = leftDistPID.getCorrection(leftmm);
 			rightSpeed = rightDistPID.getCorrection(rightmm);
 			
-			rec.addValue(leftmm,1);
 
-			if (!rec.addValue(leftSpeed,0))
+
+			///////////////////////////////////////////////////////////////////
+			// DATA LOGGGING
+			///////////////////////////////////////////////////////////////////
+
+			rec.addValue(leftDistPID.lastD_Correction, 3);
+			rec.addValue(leftDistPID.lastI_Correction, 2);
+			rec.addValue(leftDistPID.lastP_Correction,1);
+
+			if ( !rec.addValue(leftmm,0)/*leftDistPID correction*/ )
 			{
 				stopMotors();
 				rec.print();
 			}
-			
+
+			///////////////////////////////////////////////////////////////////
+
 
 
 			float speedError = actualLeftSpeed - actualRightSpeed;
@@ -369,6 +376,11 @@ namespace Micromouse
 
 			rightMotor.setMovement(rightSpeed);
 			leftMotor.setMovement(leftSpeed);
+
+
+#ifdef __MK20DX256__ // Teensy Compile
+			delay(10);
+#endif
 		}
 
 		logC(INFO) << leftDistPID.getI();

From a6538cb2282112acbf4bcc57ae0c6a189d32387b Mon Sep 17 00:00:00 2001
From: joshuasrjc <joshuasrjc@gmail.com>
Date: Tue, 9 Aug 2016 16:02:39 -0700
Subject: [PATCH 5/7] Progress

---
 micromouse/RobotIO.cpp | 58 ++++++++++++++++++++----------------------
 1 file changed, 27 insertions(+), 31 deletions(-)

diff --git a/micromouse/RobotIO.cpp b/micromouse/RobotIO.cpp
index d1ec52f..826a92d 100644
--- a/micromouse/RobotIO.cpp
+++ b/micromouse/RobotIO.cpp
@@ -165,42 +165,38 @@ namespace Micromouse
 
 	void RobotIO::testMotors()
 	{
-#ifdef __MK20DX256__ // Teensy Compile
-		//rotate(90.0f);
-		//delay(2000);
-		moveForward(6*180.0f);
+		float millimeters = 2 * 180;
+		PIDController pid = PIDController(1.0f, 0.0f, 0.1f, 1000);
+		Timer timer;
 
-/*
-		rightMotor.setMaxSpeed(0.2f);
-		leftMotor.setMaxSpeed(0.2f);
+		timer.start();
+		pid.start(millimeters);
+		while (millimeters > 1 || millimeters < -1)
+		{
+			float correction = pid.getCorrection(millimeters);
+			leftMotor.setMovement(correction);
+			rightMotor.setMovement(correction);
+			int leftCounts = leftMotor.resetCounts();
+			int rightCounts = rightMotor.resetCounts();
+			float avgMove = (leftCounts + rightCounts) / 2 / COUNTS_PER_MM;
+			millimeters -= avgMove;
+
+			float p = pid.lastP_Correction;
+			float i = pid.lastI_Correction;
+			float d = pid.lastD_Correction;
 
-		rightMotor.setMovement(1.0f);
-		delay(1000);
-		rightMotor.brake();
-		delay(1000);
-		leftMotor.setMovement(1.0f);
-		delay(1000);
-		leftMotor.brake();
-		delay(1000);
-		rightMotor.setMovement(-1.0f);
-		delay(1000);
-		rightMotor.coast();
-		delay(1000);
-		leftMotor.setMovement(-1.0f);
-		delay(1000);
-		leftMotor.coast();
-		delay(1000);
+#ifdef __MK20DX256__ // Teensy Compile
 
-		rightMotor.setMaxSpeed(1.0f);
-		leftMotor.setMaxSpeed(1.0f);
+			Serial.print(p);
+			Serial.print(", ");
+			Serial.print(i);
+			Serial.print(", ");
+			Serial.print(d);
+			Serial.print("\n");
 
-		leftMotor.setMovement(1.0f);
-		rightMotor.setMovement(1.0f);
-		delay(2000);
-		leftMotor.brake();
-		rightMotor.brake();
-		*/
+			delay(50);
 #endif
+		}
 	}
 
 

From 05aa4bca1c20b8178735e15f72176cba45ea367a Mon Sep 17 00:00:00 2001
From: Joshua Murphy <joshuasrjc@gmail.com>
Date: Tue, 9 Aug 2016 18:50:31 -0700
Subject: [PATCH 6/7] Progress 2

---
 micromouse/PIDController.cpp |  4 ++--
 micromouse/RobotIO.cpp       | 27 ++++++++++++++++++---------
 2 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/micromouse/PIDController.cpp b/micromouse/PIDController.cpp
index 1de7a50..dcab005 100644
--- a/micromouse/PIDController.cpp
+++ b/micromouse/PIDController.cpp
@@ -15,7 +15,7 @@ Author GitHub:	joshuasrjc
 
 #ifdef __MK20DX256__ // Teensy compile
 #else // PC compile
-	#include <iostream>
+#include <iostream>
 #endif
 
 
@@ -49,7 +49,7 @@ namespace Micromouse
 		float iCorrection = I * totalError;
 		float dCorrection = D * (currentError - lastError);
 
-		lastError = currentError; 
+		lastError = 0.9 * lastError + 0.1 * currentError; 
 
 		pCorrection /= 1000;
 		iCorrection /= 1000;
diff --git a/micromouse/RobotIO.cpp b/micromouse/RobotIO.cpp
index 826a92d..f49981a 100644
--- a/micromouse/RobotIO.cpp
+++ b/micromouse/RobotIO.cpp
@@ -165,20 +165,21 @@ namespace Micromouse
 
 	void RobotIO::testMotors()
 	{
-		float millimeters = 2 * 180;
-		PIDController pid = PIDController(1.0f, 0.0f, 0.1f, 1000);
+		float millimeters = 8 * 180;
+		PIDController pid = PIDController(5.0f, 4.0f, 0.5f, 200);
 		Timer timer;
 
 		timer.start();
 		pid.start(millimeters);
-		while (millimeters > 1 || millimeters < -1)
+		float avgMove = 10000000000000000000000000000000000000.0f;
+		while (millimeters > 1 || millimeters < -1 || avgMove > 1 || avgMove < -1)
 		{
 			float correction = pid.getCorrection(millimeters);
 			leftMotor.setMovement(correction);
 			rightMotor.setMovement(correction);
 			int leftCounts = leftMotor.resetCounts();
 			int rightCounts = rightMotor.resetCounts();
-			float avgMove = (leftCounts + rightCounts) / 2 / COUNTS_PER_MM;
+			avgMove = (leftCounts + rightCounts) / 2 / COUNTS_PER_MM;
 			millimeters -= avgMove;
 
 			float p = pid.lastP_Correction;
@@ -186,17 +187,25 @@ namespace Micromouse
 			float d = pid.lastD_Correction;
 
 #ifdef __MK20DX256__ // Teensy Compile
-
-			Serial.print(p);
+			Serial.print(millimeters/10, 4);
+			Serial.print(", ");
+			Serial.print(p, 4);
+			Serial.print(", ");
+			Serial.print(i, 4);
+			Serial.print(", ");
+			Serial.print(d, 4);
 			Serial.print(", ");
-			Serial.print(i);
+			Serial.print(correction, 4);
 			Serial.print(", ");
-			Serial.print(d);
+			Serial.print(leftCounts);
 			Serial.print("\n");
 
-			delay(50);
+			delay(5);
 #endif
 		}
+
+		leftMotor.brake();
+		rightMotor.brake();
 	}
 
 

From c11c8e8e26a9b4010fb1ecf08ed4e3e17a20b244 Mon Sep 17 00:00:00 2001
From: joshuasrjc <joshuasrjc@gmail.com>
Date: Sat, 13 Aug 2016 00:38:04 -0700
Subject: [PATCH 7/7] Motor Calibration

[x] Compiles on PC

[x] Compiles on Teensy

### Added
* ArduinoDummy

* A class with some Arduino dummy functions, so we don't have to use
#ifdef blocks every time we want to call a function like
digitalWrite().

* Memory
  * Addresses 0-31 in the memory class are now
used for motor calibration data. Added constants for address
locations.

* Motor
    * calibrate()
        * For each direction,
calculates the lowest voltage that still turns the motor.
    *
loadCalibration()
        * Loads the calibration data from memory.

* saveCalibration()
        * Saves the calibration data to memory.

* setVoltage()
        * Where setMovement() takes a value between -1
and 1, setVoltage() takes an int between 0 and 255. It directly sets the
PWM pin, but not lower than the minimum calibrated voltage.
* RobotIO

* calibrateMotors()
        * Calibrates the left and right motors, and
saves the data.
* Timer
    * sleep()
        * Sleeps for a given
number of seconds.
# Changed
* Controller::debug() now calls
testMotors() instead of mapMaze()
* As long as speed != 0,
Motor::setMovement() will move at the slowest calibrated speed.
*
MouseBot::robotIO is now public.

# Removed
* A bunch of #ifdef blocks
from Motor.cpp
---
 MicroMouse-2016.vcxproj         |   1 +
 MicroMouse-2016.vcxproj.filters |   3 +
 micromouse/ArduinoDummy.h       |  31 ++++++++
 micromouse/Controller.cpp       |   8 +--
 micromouse/Memory.h             |  17 ++++-
 micromouse/Motor.cpp            | 123 ++++++++++++++++++++++++++++----
 micromouse/Motor.h              |  24 +++++++
 micromouse/MouseBot.h           |   3 +-
 micromouse/RobotIO.cpp          |  15 +++-
 micromouse/RobotIO.h            |  20 +++---
 micromouse/Timer.cpp            |  15 ++++
 micromouse/Timer.h              |   2 +
 12 files changed, 234 insertions(+), 28 deletions(-)
 create mode 100644 micromouse/ArduinoDummy.h

diff --git a/MicroMouse-2016.vcxproj b/MicroMouse-2016.vcxproj
index 4570998..11a15a2 100644
--- a/MicroMouse-2016.vcxproj
+++ b/MicroMouse-2016.vcxproj
@@ -166,6 +166,7 @@
     <ClCompile Include="micromouse\VirtualMaze.cpp" />
   </ItemGroup>
   <ItemGroup>
+    <ClInclude Include="micromouse\ArduinoDummy.h" />
     <ClInclude Include="micromouse\ButtonFlag.h" />
     <ClInclude Include="micromouse\Controller.h" />
     <ClInclude Include="micromouse\FlagMatrix.h" />
diff --git a/MicroMouse-2016.vcxproj.filters b/MicroMouse-2016.vcxproj.filters
index 60ee33f..0c4fa38 100644
--- a/MicroMouse-2016.vcxproj.filters
+++ b/MicroMouse-2016.vcxproj.filters
@@ -113,5 +113,8 @@
     <ClInclude Include="micromouse\Recorder.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="micromouse\ArduinoDummy.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/micromouse/ArduinoDummy.h b/micromouse/ArduinoDummy.h
new file mode 100644
index 0000000..33d64fe
--- /dev/null
+++ b/micromouse/ArduinoDummy.h
@@ -0,0 +1,31 @@
+#pragma once
+
+namespace Micromouse
+{
+#ifndef __MK20DX256__ // If NOT on Teensy
+
+	enum PinMode
+	{
+		INPUT, OUTPUT
+	};
+
+	enum PinValue
+	{
+		LOW, HIGH
+	};
+
+	void pinMode(int pin, PinMode mode) { }
+	void digitalWrite(int pin, PinValue value) { }
+	void analogWrite(int pin, int value) { }
+
+
+	class SERIAL
+	{
+	public:
+		void println(float x) {}
+	};
+
+	SERIAL Serial = SERIAL();
+
+#endif
+}
\ No newline at end of file
diff --git a/micromouse/Controller.cpp b/micromouse/Controller.cpp
index 866e324..57c0155 100644
--- a/micromouse/Controller.cpp
+++ b/micromouse/Controller.cpp
@@ -34,10 +34,10 @@ namespace Micromouse
 	{
 		// DEBUG CODE GOES IN HERE!
 
-		mouse.mapMaze();
+		//mouse.mapMaze();
 		//mouse.runMaze();
 		//mouse.testIR();
-		//mouse.testMotors();
+		mouse.testMotors();
 		//mouse.testRotate();
 
 		// DEBUG CODE GOES IN HERE!
@@ -95,7 +95,7 @@ namespace Micromouse
 			"1: map maze\n"
 			"2: run maze\n"
 			"3: cycle speed\n"
-			"4: nothing\n"
+			"4: debug\n"
 			"5: calibrate sensors\n"
 			"6: calibrate motor\n"
 			"7: reset Map\n";
@@ -209,7 +209,7 @@ namespace Micromouse
 
 			blinkLED(CAL_MOTOR);
 			state = NONE;
-			//TODO decide if this is even something we need
+			mouse.robotIO.calibrateMotors();
 		break;
 
 
diff --git a/micromouse/Memory.h b/micromouse/Memory.h
index e645721..c3d616e 100644
--- a/micromouse/Memory.h
+++ b/micromouse/Memory.h
@@ -6,10 +6,15 @@
 
 namespace Micromouse {
 
-	//0-99 unreserved
+	/*
+	0 - 31: Motor Calibration Data Block
+	*/
+
+	//32-99 unreserved
 
 
 	/* 
+
 	100 - 179: IRSensor Calibration Data Block 
 	relative to the starting address
 	0: calibrationSize
@@ -17,6 +22,7 @@ namespace Micromouse {
 	2: calibrationInterval
 	3-19: CalibrationData[]
 	*/
+
 	const int IR_FRONT_LEFT_MEMORY = 100;//-119
 	const int IR_FRONT_RIGHT_MEMORY = 200;//-139
 	const int IR_LEFT_MEMORY = 300;//-159
@@ -32,6 +38,15 @@ namespace Micromouse {
 	class Memory
 	{
 	public:
+		static const int RIGHT_FWD_VOLTAGE = 0;
+		static const int RIGHT_BWD_VOLTAGE = 4;
+		static const int RIGHT_FWD_SPEED = 8;
+		static const int RIGHT_BWD_SPEED = 12;
+		static const int LEFT_FWD_VOLTAGE = 16;
+		static const int LEFT_BWD_VOLTAGE = 20;
+		static const int LEFT_FWD_SPEED = 24;
+		static const int LEFT_BWD_SPEED = 28;
+
 		static int read(int address);
 		static void write(int address, int val);
 
diff --git a/micromouse/Motor.cpp b/micromouse/Motor.cpp
index f77fef0..3f4cf38 100644
--- a/micromouse/Motor.cpp
+++ b/micromouse/Motor.cpp
@@ -1,5 +1,9 @@
 #include "Motor.h"
 #include "Logger.h"
+#include "Timer.h"
+#include "ArduinoDummy.h"
+#include "Memory.h"
+#include "RobotIO.h"
 
 namespace Micromouse
 {
@@ -11,25 +15,102 @@ namespace Micromouse
 		, encoder(Encoder(fwdEncoderPin, bwdEncoderPin))
 #endif
 	{
+		loadCalibration();
 		initPins();
 	}
 
 
+	
+	void Motor::loadCalibration()
+	{
+		if (pwmPin == MOTOR_LEFT_PWM_PIN)
+		{
+			minFwdVoltage = Memory::read(Memory::LEFT_FWD_VOLTAGE);
+			minBwdVoltage = Memory::read(Memory::LEFT_BWD_VOLTAGE);
+		}
+		else if (pwmPin == MOTOR_RIGHT_PWM_PIN)
+		{
+			minFwdVoltage = Memory::read(Memory::RIGHT_FWD_VOLTAGE);
+			minBwdVoltage = Memory::read(Memory::RIGHT_BWD_VOLTAGE);
+		}
+	}
+
+
+
+	void Motor::saveCalibration()
+	{
+		if (pwmPin == MOTOR_LEFT_PWM_PIN)
+		{
+			Memory::write(Memory::LEFT_FWD_VOLTAGE, minFwdVoltage);
+			Memory::write(Memory::LEFT_BWD_VOLTAGE, minBwdVoltage);
+		}
+		else if (pwmPin == MOTOR_RIGHT_PWM_PIN)
+		{
+			Memory::write(Memory::RIGHT_FWD_VOLTAGE, minFwdVoltage);
+			Memory::write(Memory::RIGHT_BWD_VOLTAGE, minBwdVoltage);
+		}
+	}
+
+
 
 	void Motor::initPins()
 	{
-#ifdef __MK20DX256__ // Teensy Compile
 		pinMode(fwdPin, OUTPUT);
 		pinMode(bwdPin, OUTPUT);
 		pinMode(pwmPin, OUTPUT);
-#endif
+	}
+
+
+
+	void Motor::calibrate()
+	{
+		digitalWrite(fwdPin, HIGH);
+		digitalWrite(bwdPin, LOW);
+		minFwdVoltage = calibrateMinVoltage() + 3;
+
+		digitalWrite(fwdPin, LOW);
+		digitalWrite(bwdPin, HIGH);
+		minBwdVoltage = calibrateMinVoltage() + 3;
+
+		digitalWrite(fwdPin, LOW);
+		digitalWrite(bwdPin, LOW);
+
+		saveCalibration();
+	}
+
+
+
+	int Motor::calibrateMinVoltage()
+	{
+		Timer timer = Timer();
+		int voltage = 64;
+		bool timedOut = false;
+		while (voltage > 0 && !timedOut)
+		{
+			voltage--;
+			analogWrite(pwmPin, voltage);
+
+			float waitTime = 0;
+			timer.start();
+			resetCounts();
+			while (getCounts() < 10 && getCounts() > -10 && !timedOut)
+			{
+				waitTime += timer.getDeltaTime();
+				Serial.println(waitTime);
+				if (waitTime >= CALIBRATION_TIMEOUT_SEC)
+				{
+					timedOut = true;
+				}
+			}
+		}
+
+		return voltage;
 	}
 
 
 
 	void Motor::setMovement(float speed)
 	{
-#ifdef __MK20DX256__ // Teensy Compile
 		if (speed > 1 || speed < -1)
 		{
 			logC(WARN) << "In setMovement, speed was not between -1 and 1";
@@ -42,29 +123,51 @@ namespace Micromouse
 		{
 			digitalWrite(fwdPin, HIGH);
 			digitalWrite(bwdPin, LOW);
-			analogWrite(pwmPin, (int)(255 * speed));
+			setVoltage((int)(255 * speed), true);
 		}
 		else
 		{
 			digitalWrite(fwdPin, LOW);
 			digitalWrite(bwdPin, HIGH);
-			analogWrite(pwmPin, (int)(255 * (-speed)));
+			setVoltage((int)(255 * (-speed)), false);
 		}
-#endif
+	}
+
+
+	
+	void Motor::setVoltage(int voltage, bool fwd)
+	{
+		if (voltage > 0)
+		{
+			if (fwd)
+			{
+				if (voltage < minFwdVoltage)
+				{
+					voltage = minFwdVoltage;
+				}
+			}
+			else
+			{
+				if (voltage < minBwdVoltage)
+				{
+					voltage = minBwdVoltage;
+				}
+			}
+		}
+
+		analogWrite(pwmPin, voltage);
 	}
 
 
 
 	void Motor::setMaxSpeed(float maxSpeed)
 	{
-#ifdef __MK20DX256__ // Teensy Compile
 		if (maxSpeed < 0 || maxSpeed > 1)
 		{
 			logC(WARN) << "Motor was given a maxSpeed that is not between 0 and 1.";
 			maxSpeed = maxSpeed < 0 ? 0 : 1;
 		}
 		this->maxSpeed = maxSpeed;
-#endif
 	}
 
 
@@ -78,22 +181,18 @@ namespace Micromouse
 
 	void Motor::brake()
 	{
-#ifdef __MK20DX256__ // Teensy Compile
 		digitalWrite(fwdPin, HIGH);
 		digitalWrite(bwdPin, HIGH);
 		analogWrite(pwmPin, 0);
-#endif
 	}
 
 
 
 	void Motor::coast()
 	{
-#ifdef __MK20DX256__ // Teensy Compile
 		digitalWrite(fwdPin, LOW);
 		digitalWrite(bwdPin, LOW);
 		analogWrite(pwmPin, 0);
-#endif
 	}
 	
 
diff --git a/micromouse/Motor.h b/micromouse/Motor.h
index 542e5ac..b05503b 100644
--- a/micromouse/Motor.h
+++ b/micromouse/Motor.h
@@ -1,5 +1,7 @@
 #pragma once
 
+//#include <string>
+
 #ifdef __MK20DX256__ // Teensy Compile
 	#include <Encoder.h>
 #endif
@@ -11,8 +13,20 @@ namespace Micromouse
 	class Motor
 	{
 	public:
+		const float CALIBRATION_TIMEOUT_SEC = 0.5f;
+
 		Motor(int fwdPin, int bwdPin, int pwmPin, int fwdEncoderPin, int bwdEncoderPin);
 
+		//Loads calibration data from memory.
+		void loadCalibration();
+
+		//Saves calibration data to memory.
+		void saveCalibration();
+
+		//Spins the motor forward and backward, calibrating minFwdVoltage and minBwdVoltage,
+		//which are each set to the lowest speed that still turns the motor.
+		void calibrate();
+
 		//speed should be a value between -1 and 1.
 		//Positive speeds move the motor forward. Negative speeds move it backward.
 		//The actual speed of the motor will be the product of speed and maxSpeed ( see setMaxSpeed()).
@@ -38,11 +52,21 @@ namespace Micromouse
 		//Returns the number of encoder counts since resetCounts() was last called.
 		//Then resets the encoder count.
 		int resetCounts();
+
 	private:
 		void initPins();
 
+		int calibrateMinVoltage();
+
+		void setVoltage(int voltage, bool fwd);
+
 		float maxSpeed = 1.0f;
 
+		//The minimum speed that still turns the motor (forward)
+		int minFwdVoltage = 0;
+		//The minimum speed that still turns the motor (backward)
+		int minBwdVoltage = 0;
+
 		int fwdPin;			//Forward pin
 		int bwdPin;			//Backward pin
 		int pwmPin;			//Pulse-width modulation pin (for speed control)
diff --git a/micromouse/MouseBot.h b/micromouse/MouseBot.h
index ec036fd..8f91bbf 100644
--- a/micromouse/MouseBot.h
+++ b/micromouse/MouseBot.h
@@ -27,6 +27,8 @@ namespace Micromouse
 	class MouseBot
 	{
 	public:
+		RobotIO robotIO;
+
 		MouseBot(int x = 0, int y = 0);					// Sets the position to (x,y)
 		~MouseBot();
 
@@ -92,7 +94,6 @@ namespace Micromouse
 		VirtualMaze* virtualMaze;
 #endif
 		int saveAddress = 512;
-		RobotIO robotIO;
 		std::stack<direction> movementHistory;
 
 		// Mouse position in the maze
diff --git a/micromouse/RobotIO.cpp b/micromouse/RobotIO.cpp
index f49981a..7c5a867 100644
--- a/micromouse/RobotIO.cpp
+++ b/micromouse/RobotIO.cpp
@@ -165,6 +165,12 @@ namespace Micromouse
 
 	void RobotIO::testMotors()
 	{
+		leftMotor.setMovement(0.004f);
+		rightMotor.setMovement(0.004f);
+		Timer::sleep(2.0f);
+		leftMotor.brake();
+		rightMotor.brake();
+		/*
 		float millimeters = 8 * 180;
 		PIDController pid = PIDController(5.0f, 4.0f, 0.5f, 200);
 		Timer timer;
@@ -205,7 +211,7 @@ namespace Micromouse
 		}
 
 		leftMotor.brake();
-		rightMotor.brake();
+		rightMotor.brake(); */
 	}
 
 
@@ -595,6 +601,13 @@ namespace Micromouse
 	}
 
 
+	void RobotIO::calibrateMotors()
+	{
+		rightMotor.calibrate();
+		leftMotor.calibrate();
+	}
+
+
 	void RobotIO::stopMotors()
 	{
 		leftMotor.brake();
diff --git a/micromouse/RobotIO.h b/micromouse/RobotIO.h
index 2a579b6..e122920 100644
--- a/micromouse/RobotIO.h
+++ b/micromouse/RobotIO.h
@@ -88,6 +88,8 @@ namespace Micromouse
 
 		void calibrateIRSensors();
 
+		void calibrateMotors();
+
 		void stopMotors();
 
 	private:
@@ -100,15 +102,6 @@ namespace Micromouse
 
 		IRSensor* IRSensors[4];
 
-		Motor rightMotor = Motor
-		(
-			MOTOR_RIGHT_FWD_PIN,
-			MOTOR_RIGHT_BWD_PIN,
-			MOTOR_RIGHT_PWM_PIN,
-			ENCODER_RIGHT_FWD_PIN,
-			ENCODER_RIGHT_BWD_PIN
-		);
-
 		Motor leftMotor = Motor
 		(
 			MOTOR_LEFT_FWD_PIN,
@@ -117,5 +110,14 @@ namespace Micromouse
 			ENCODER_LEFT_FWD_PIN,
 			ENCODER_LEFT_BWD_PIN
 		);
+
+		Motor rightMotor = Motor
+		(
+			MOTOR_RIGHT_FWD_PIN,
+			MOTOR_RIGHT_BWD_PIN,
+			MOTOR_RIGHT_PWM_PIN,
+			ENCODER_RIGHT_FWD_PIN,
+			ENCODER_RIGHT_BWD_PIN
+		);
 	};
 }
\ No newline at end of file
diff --git a/micromouse/Timer.cpp b/micromouse/Timer.cpp
index 723a4d5..ea5b40f 100644
--- a/micromouse/Timer.cpp
+++ b/micromouse/Timer.cpp
@@ -6,6 +6,21 @@
 
 namespace Micromouse
 {
+	void Timer::sleep(float seconds)
+	{
+		Timer timer = Timer();
+		for (
+			float sleepTime = 0;
+			sleepTime < seconds;
+			sleepTime += timer.getDeltaTime()
+			)
+		{
+			// Wait
+		}
+	}
+
+
+
 	Timer::Timer()
 	{
 		start();
diff --git a/micromouse/Timer.h b/micromouse/Timer.h
index 0993b5e..ab2ed6f 100644
--- a/micromouse/Timer.h
+++ b/micromouse/Timer.h
@@ -9,6 +9,8 @@ namespace Micromouse
 	class Timer
 	{
 	public:
+		static void sleep(float seconds);
+
 		Timer();
 
 		void start();