Motor Calibration

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" />
@@ -177,6 +178,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" />

MicroMouse-2016.vcxproj.filters

@@ -110,5 +110,11 @@
     <ClInclude Include="micromouse\ButtonFlag.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="micromouse\Recorder.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="micromouse\ArduinoDummy.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

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

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;
 
 

micromouse/Memory.h

@@ -6,17 +6,23 @@
 
 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
 	1: calibrationStart
 	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);
 

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
 	}
 
 

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)

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