diff --git a/MicroMouse-2016.vcxproj b/MicroMouse-2016.vcxproj
index a784729..11a15a2 100644
--- a/MicroMouse-2016.vcxproj
+++ b/MicroMouse-2016.vcxproj
@@ -166,6 +166,7 @@
+
@@ -177,6 +178,7 @@
+
diff --git a/MicroMouse-2016.vcxproj.filters b/MicroMouse-2016.vcxproj.filters
index e0fe22e..0c4fa38 100644
--- a/MicroMouse-2016.vcxproj.filters
+++ b/MicroMouse-2016.vcxproj.filters
@@ -110,5 +110,11 @@
Header Files
+
+ Header Files
+
+
+ Header Files
+
\ 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
+
#ifdef __MK20DX256__ // Teensy Compile
#include
#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 movementHistory;
// Mouse position in the maze
diff --git a/micromouse/PIDController.cpp b/micromouse/PIDController.cpp
index d2232ee..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
+#include
#endif
@@ -49,8 +49,17 @@ namespace Micromouse
float iCorrection = I * totalError;
float dCorrection = D * (currentError - lastError);
+ lastError = 0.9 * lastError + 0.1 * 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);
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
+ 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
+ Recorder::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
+ Recorder::~Recorder()
+ {
+ for (size_t i = 0; i < DIMENSION; i++)
+ {
+ delete[] arrs[i];
+ }
+
+ delete[] arrs;
+ delete[] ind;
+ }
+
+
+ template
+ bool Recorder::addValue(T val, int dimension)
+ {
+ if (ind[dimension] < SIZE)
+ {
+ arrs[dimension][ind[dimension]++] = val;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ template
+ void Recorder::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 91bda74..7c5a867 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"
@@ -164,47 +165,53 @@ namespace Micromouse
void RobotIO::testMotors()
{
-#ifdef __MK20DX256__ // Teensy Compile
- //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);
-
- rightMotor.setMovement(1.0f);
- delay(1000);
- rightMotor.brake();
- delay(1000);
- leftMotor.setMovement(1.0f);
- delay(1000);
+ leftMotor.setMovement(0.004f);
+ rightMotor.setMovement(0.004f);
+ Timer::sleep(2.0f);
leftMotor.brake();
- delay(1000);
- rightMotor.setMovement(-1.0f);
- delay(1000);
- rightMotor.coast();
- delay(1000);
- leftMotor.setMovement(-1.0f);
- delay(1000);
- leftMotor.coast();
- delay(1000);
+ rightMotor.brake();
+ /*
+ float millimeters = 8 * 180;
+ PIDController pid = PIDController(5.0f, 4.0f, 0.5f, 200);
+ Timer timer;
- rightMotor.setMaxSpeed(1.0f);
- leftMotor.setMaxSpeed(1.0f);
+ timer.start();
+ pid.start(millimeters);
+ 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();
+ avgMove = (leftCounts + rightCounts) / 2 / COUNTS_PER_MM;
+ millimeters -= avgMove;
+
+ float p = pid.lastP_Correction;
+ float i = pid.lastI_Correction;
+ float d = pid.lastD_Correction;
- leftMotor.setMovement(1.0f);
- rightMotor.setMovement(1.0f);
- delay(2000);
- leftMotor.brake();
- rightMotor.brake();
- */
+#ifdef __MK20DX256__ // Teensy Compile
+ 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(correction, 4);
+ Serial.print(", ");
+ Serial.print(leftCounts);
+ Serial.print("\n");
+
+ delay(5);
#endif
+ }
+
+ leftMotor.brake();
+ rightMotor.brake(); */
}
@@ -239,17 +246,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 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);
@@ -288,6 +297,7 @@ namespace Micromouse
BUTTONFLAG
float deltaTime = timer.getDeltaTime();
+
//Get distance from the front of the bot to the wall.
frontRightIRDist = IRSensors[FRONT_RIGHT]->getDistance();
@@ -296,6 +306,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,8 +318,29 @@ namespace Micromouse
leftmm -= leftTraveled;
rightmm -= rightTraveled;
+
leftSpeed = leftDistPID.getCorrection(leftmm);
rightSpeed = rightDistPID.getCorrection(rightmm);
+
+
+
+ ///////////////////////////////////////////////////////////////////
+ // 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;
float speedCorrection = speedPID.getCorrection(speedError);
@@ -354,6 +387,11 @@ namespace Micromouse
rightMotor.setMovement(rightSpeed);
leftMotor.setMovement(leftSpeed);
+
+
+#ifdef __MK20DX256__ // Teensy Compile
+ delay(10);
+#endif
}
logC(INFO) << leftDistPID.getI();
@@ -364,6 +402,8 @@ namespace Micromouse
leftMotor.brake();
rightMotor.brake();
+ rec.print();
+
}
@@ -559,4 +599,18 @@ namespace Micromouse
IRSensors[FRONT_LEFT]->saveCalibration(IR_FRONT_LEFT_MEMORY);
IRSensors[FRONT_RIGHT]->saveCalibration(IR_FRONT_RIGHT_MEMORY);
}
+
+
+ void RobotIO::calibrateMotors()
+ {
+ rightMotor.calibrate();
+ leftMotor.calibrate();
+ }
+
+
+ void RobotIO::stopMotors()
+ {
+ leftMotor.brake();
+ rightMotor.brake();
+ }
}
diff --git a/micromouse/RobotIO.h b/micromouse/RobotIO.h
index bca4563..e122920 100644
--- a/micromouse/RobotIO.h
+++ b/micromouse/RobotIO.h
@@ -88,6 +88,10 @@ namespace Micromouse
void calibrateIRSensors();
+ void calibrateMotors();
+
+ void stopMotors();
+
private:
enum IRDirection { LEFT, RIGHT, FRONT_LEFT, FRONT_RIGHT };
@@ -98,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,
@@ -115,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();