Merge branch 'dev' into benji_led-class-#2

.gitignore

@@ -50,7 +50,7 @@ __vm/
 *.user
 *.userosscache
 *.sln.docstates
-*.vcxproj.filters
+#####*.vcxproj.filters
 
 # User-specific files (MonoDevelop/Xamarin Studio)
 *.userprefs

AveragingQueue.h

@@ -0,0 +1,108 @@
+#pragma once
+
+#include <queue>
+
+namespace LFRobot
+{
+	/*
+	AveragingQueue functions as a normal queue, with two added features.
+
+	1. It keeps track of the average of all the values in the queue, 
+	so that it can be found in constant time.
+
+	2. It has a maximum size, called capacity. When the capacity is reached,
+	the oldest element is removed from the queue and from the average.
+
+	If capcity is zero, then the queue has unlimited size.
+	*/
+	template <class T>
+	class AveragingQueue
+	{
+	public:
+		/*
+		Initializes an empty queue with unlimited size.
+		*/
+		AveragingQueue()
+		{
+			capacity = 0;
+		}
+
+		/*
+		Initializes an empty queue with a given maximum size.
+		*/
+		AveragingQueue(size_t inCapacity)
+		{
+			capacity = inCapacity;
+		}
+
+		/*
+		Pushes a new element onto the queue and into the average.
+		If the queue is full, the oldest element is removed.
+		*/
+		void push(T element)
+		{
+			sum += element;
+			q.push(element);
+
+			if (capacity > 0 && q.size() > capacity)
+			{
+				sum -= q.front();
+				q.pop();
+			}
+		}
+
+		/*
+		Removes the oldest element from the queue and from the average.
+		That element is returned.
+		*/
+		T pop()
+		{
+			T temp = q.front();
+			sum -= temp;
+			q.pop();
+			return temp;
+		}
+
+		/*
+		Returns the average of all the elements in the queue.
+		*/
+		T getAverage()
+		{
+			if (q.empty())
+			{
+				return 0;
+			}
+
+			return sum / q.size();
+		}
+
+		/*
+		Returns the number of elements currently in the queue.
+		*/
+		size_t getSize()
+		{
+			return q.size();
+		}
+
+		/*
+		True if the capacity is limited and the size is equal to it.
+		*/
+		bool isFull()
+		{
+			return capacity > 0 && q.size() == capacity;
+		}
+
+		/*
+		True if there are no elements in the queue.
+		*/
+		bool isEmpty()
+		{
+			return q.empty();
+		}
+
+	private:
+		std::queue<T> q;
+		size_t capacity;
+		T sum = 0;
+	};
+}

Constants.h

@@ -0,0 +1,22 @@
+#pragma once
+
+const int PIN_LED = 13;
+const int N_SENSORS = 8;
+
+//										{ 0,   1,   2,   3,   4,   5,   6,   7  }
+const int PIN_SENSORS[N_SENSORS] =		{ 20,  19,  18,  17,  16,  15,  11,  12 };
+const long MICROS_WHITE[N_SENSORS] =	{ 280, 200, 170, 160, 160, 170, 200, 250 };
+const long MICROS_BLACK[N_SENSORS] =	{ 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000 };
+const long MICROS_TIMEOUT = 3000;
+//motor pins left
+const int PIN_MOTOR_LEFT_FORWARD = 5;
+const int PIN_MOTOR_LEFT_BACKWARD = 6;
+const int PIN_MOTOR_LEFT_PWM = 4;
+//motor pins right
+const int PIN_MOTOR_RIGHT_FORWARD = 8;
+const int PIN_MOTOR_RIGHT_BACKWARD = 9;
+const int PIN_MOTOR_RIGHT_PWM = 10;
+//
+const float ROBOT_SPEED = 1;
+const int VALUES_STORED = 3;
+const float MIN_LINE_VALUE = 0.05;

LineFollower.cpp

@@ -0,0 +1,44 @@
+#include "LineFollower.h"
+#include "SensorArray.h"
+#include "PIDController.h"
+
+
+namespace LFRobot
+{
+	LineFollower::LineFollower()
+	{
+		sensorArray = new SensorArray(PIN_SENSORS, N_SENSORS, MICROS_WHITE, MICROS_BLACK, MICROS_TIMEOUT);
+		motors = new MotorPair();
+	}
+
+	LineFollower::~LineFollower()
+	{
+		delete sensorArray;
+		delete motors;
+	}
+
+	void LineFollower::followLine()
+	{
+		PIDController controller(1.0f, 0.0f, 10.0f);
+
+		controller.start(0);
+
+		while (true)
+		{
+			float error = sensorArray->getLineOffset();
+
+			float correction = controller.getCorrection(error);
+
+			motors->moveForward(ROBOT_SPEED, correction);
+		}
+	}
+
+	void LineFollower::testSensorArray()
+	{
+		while (true)
+		{
+			Serial.println(sensorArray->getLineOffset());
+			delay(100);
+		}
+	}
+}

LineFollower.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "Constants.h"
+#include "SensorArray.h"
+#include "MotorPair.h"
+
+namespace LFRobot
+{
+	class LineFollower
+	{
+	public:
+		LineFollower();
+		~LineFollower();
+
+		void followLine();
+
+		void testSensorArray();
+	private:
+		SensorArray* sensorArray;
+		MotorPair* motors;
+	};
+}

Motor.cpp

@@ -0,0 +1,46 @@
+#include "arduino.h"
+#include "Math.h"
+#include "Motor.h"
+
+
+Motor::Motor(int set_forward_pin, int set_backward_pin, int set_pwm_pin)
+	: FORWARD_PIN(set_forward_pin), BACKWARD_PIN(set_backward_pin), PWM_PIN(set_pwm_pin)
+{
+	pinMode(FORWARD_PIN, OUTPUT);
+	pinMode(BACKWARD_PIN, OUTPUT);
+	pinMode(PWM_PIN, OUTPUT);
+}
+
+
+
+void Motor::coast() {
+	digitalWrite(FORWARD_PIN, LOW);
+	digitalWrite(BACKWARD_PIN, LOW);
+}
+
+void Motor::setSpeed(float speed)
+{
+	//Clamps speed between -1 and 1
+	speed = (speed > 1) ? (1) : (speed);
+	speed = (speed < -1) ? (-1) : (speed);
+
+	if (speed == 0)
+	{
+		coast();
+	}
+	else if (speed > 0)
+	{
+		analogWrite(PWM_PIN, 255.0f * speed);
+
+		digitalWrite(FORWARD_PIN, HIGH);
+		digitalWrite(BACKWARD_PIN, LOW);
+
+	}else{
+
+		//maps 0 to 1 to a value between 0 and 255
+		analogWrite(PWM_PIN, 255.0f * (-speed));
+
+		digitalWrite(FORWARD_PIN, LOW);
+		digitalWrite(BACKWARD_PIN, HIGH);
+	}
+}

Motor.h

@@ -0,0 +1,19 @@
+#pragma once
+#include "Constants.h"
+
+class Motor
+{
+
+public:
+	Motor(int set_forward_pin, int set_backward_pin, int set_pwm_pin);
+
+	void coast();
+	void setSpeed(float speed);
+private:
+	float speed;
+	int FORWARD_PIN;
+	int BACKWARD_PIN;
+	int PWM_PIN;
+
+
+};

MotorPair.cpp

@@ -0,0 +1,28 @@
+#include "MotorPair.h"
+
+
+MotorPair::MotorPair()
+	: rightMotor(PIN_MOTOR_RIGHT_FORWARD, PIN_MOTOR_RIGHT_BACKWARD, PIN_MOTOR_RIGHT_PWM), 
+	leftMotor(PIN_MOTOR_LEFT_FORWARD, PIN_MOTOR_LEFT_BACKWARD, PIN_MOTOR_LEFT_PWM)
+{
+
+}
+
+void MotorPair::moveForward(float speed, float turning)
+{
+	turning = turning > 2 ? 2 : turning;
+	turning = turning < -2 ? -2 : turning;
+
+	if (turning >= 0)
+	{
+		//Turn right!
+		leftMotor.setSpeed(speed);
+		rightMotor.setSpeed(speed * (1 - turning));
+	}
+	else
+	{
+		//Turn left!
+		leftMotor.setSpeed(speed * (1 + turning));
+		rightMotor.setSpeed(speed);
+	}
+}

MotorPair.h

@@ -0,0 +1,18 @@
+#pragma once
+#include "Motor.h"
+#include "Constants.h"
+
+class MotorPair
+{
+public:
+	MotorPair();
+	void moveForward(float speed, float turning);
+
+private:
+	Motor leftMotor;
+	Motor rightMotor;
+};
+
+
+
+

PIDController.cpp

@@ -0,0 +1,43 @@
+#include "PIDController.h"
+
+namespace LFRobot
+{
+
+	PIDController::PIDController(float P, float I, float D)
+		: Kp(P), Ki(I), Kd(D)
+	{
+
+	}
+
+	void PIDController::start(const float inError) {
+		pastError.push(inError);
+		prevTime = micros();
+		firstError = false;
+	}
+
+	float PIDController::getCorrection(float error) {
+
+		if (firstError)
+		{
+			start(error);
+		}
+
+		float pTerm, iTerm, dTerm;
+
+		long currentTime = micros();
+
+		float deltaTime = currentTime - prevTime;
+		deltaTime /= 1000; //Convert us to ms.
+		integralTotal += error * deltaTime;
+
+		pTerm = Kp * error;
+		iTerm = Ki * integralTotal;
+		dTerm = Kd * (error - pastError.getAverage()) / deltaTime;
+
+		pastError.push(error);
+		prevTime = currentTime;
+
+		return pTerm + iTerm + dTerm;
+	}
+
+}

PIDController.h

@@ -0,0 +1,24 @@
+#include <Arduino.h>
+#include "AveragingQueue.h"
+
+
+#pragma once
+
+namespace LFRobot {
+	class PIDController {
+	public:
+		PIDController(float P, float I, float D);
+		void start(const float inError);
+		float getCorrection(float error);
+	private:
+
+		float Kp, Ki, Kd;
+
+		long prevTime;
+
+		AveragingQueue<float> pastError = AveragingQueue<float>(3);
+		float integralTotal = 0;
+
+		bool firstError = true;
+	};
+}