Added new PID algorithm

Author: j54n1n

avr/libraries/LottieLemon/src/utility/LottieLemonLineFollow.cpp

@@ -20,151 +20,75 @@
 #include "LottieLemonLineFollow.h"
 using namespace LottieLemon;
 
-//#define KP 19 //0.1 units
-//#define KD 14
-//#define ROBOT_SPEED 100 //percentage
-
-//#define KP 11
-//#define KD 5
-//#define ROBOT_SPEED 50
-
-//#define INTEGRATION_TIME 10 //En ms
-
-/*uint8_t KP=11;
-uint8_t KD=5;
-uint8_t robotSpeed=50; //percentage
-uint8_t intergrationTime=10;*/
-
-#define NIVEL_PARA_LINEA 50 
-
-/*int lectura_sensor[5], last_error=0, acu=0;
-
-//Estos son los arrays que hay que rellenar con los valores de los sensores
-//de suelo sobre blanco y negro.
-int sensor_blanco[]={
-0,0,0,0,0};
-int sensor_negro[]={
-1023,1023,1023,1023,1023};
-*/
-//unsigned long time;
-
-//void mueve_robot(int vel_izq, int vel_der);
-//void para_robot();
-//void doCalibration(int speedPct, int time);
-//void ajusta_niveles();	//calibrate values
-
 LineFollow::LineFollow() {
-	/*KP=11;
-	KD=5;
-	robotSpeed=50; //percentage
-	intergrationTime=10;*/
 	config(11, 5, 50, 10);
-
-	for (int i = 0; i<5; i++) {
-		sensor_blanco[i] = 0;
-		sensor_negro[i] = 1023;
-	}
 }
 
-void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime) {
-	this->KP = KP;
-	this->KD = KD;
-	this->robotSpeed = robotSpeed;
-	this->intergrationTime = intergrationTime;
-	/*Serial.print("LFC: ");
-	Serial.print(KP);
-	Serial.print(' ');
-	Serial.print(KD);
-	Serial.print(' ');
-	Serial.print(robotSpeed);
-	Serial.print(' ');
-	Serial.println(intergrationTime);*/
+void LineFollow::config(uint8_t KP, uint8_t KD,
+	uint8_t robotSpeedPct, uint8_t integrationTimeMillis) {
+	config(KP, 0, KD, robotSpeedPct, integrationTimeMillis);
+}
 
+void LineFollow::config(uint8_t KP, uint8_t KI, uint8_t KD,
+	uint8_t robotSpeedPct, uint8_t integrationTimeMillis) {
+	_KP = KP;
+	_KI = KI;
+	_KD = KD;
+	_robotSpeedPct = robotSpeedPct;
+	_integrationTimeMillis = integrationTimeMillis;
 }
-void LineFollow::calibIRs() {
-	static bool isInited = false;//So only init once
-	if (isInited)return;
 
+void LineFollow::calibIRs() {
+	// So only init once.
+	static bool isInited = false;
+	if (isInited) {
+		return;
+	}
 	delay(1000);
-
 	doCalibration(30, 500);
 	doCalibration(-30, 800);
 	doCalibration(30, 500);
-
 	delay(1000);
 	isInited = true;
 }
 
 void LineFollow::runLineFollow() {
-	for (int count = 0; count<5; count++)
-	{
-		lectura_sensor[count] = map(_IRread(count), sensor_negro[count], sensor_blanco[count], 0, 127);
-		acu += lectura_sensor[count];
-	}
-
-	//Serial.println(millis());
-	if (acu > NIVEL_PARA_LINEA)
-	{
-		acu /= 5;
-
-		int error = ((lectura_sensor[0] << 6) + (lectura_sensor[1] << 5) - (lectura_sensor[3] << 5) - (lectura_sensor[4] << 6)) / acu;
-
-		error = constrain(error, -100, 100);
-
-		//Calculamos la correcion de velocidad mediante un filtro PD
-		int vel = (error * KP) / 10 + (error - last_error)*KD;
-
-		last_error = error;
-
-		//Corregimos la velocidad de avance con el error de salida del filtro PD
-		int motor_left = constrain((robotSpeed + vel), -100, 100);
-		int motor_right = constrain((robotSpeed - vel), -100, 100);
-
-		//Movemos el robot
-		//motorsWritePct(motor_left,motor_right);
-		motorsWritePct(motor_left, motor_right);
-
-		//Esperamos un poquito a que el robot reaccione
-		delay(intergrationTime);
+	enum { STOP_LINE_VALUE = 1500 };
+	uint32_t weight = 0;
+	uint16_t sum = 0;
+	for (int i = 0; i < 5; i++) {
+		_sensors[i] = _IRread(i);
+		weight += _sensors[i] * (i + 1);
+		sum += _sensors[i];
 	}
-	else
-	{
-		//Hemos encontrado una linea negra
-		//perpendicular a nuestro camino
-		//paramos el robot
+	if (sum > STOP_LINE_VALUE) {
+		if ((millis() - _tStartMillis) <= _integrationTimeMillis) {
+			return;
+		}
+		_tStartMillis = millis();
+		int16_t error = 100 * weight / sum - 300;
+		_integral = _integral + error;
+		_derivative = error - _lastError;
+		_lastError = error;
+		// Calculate the adjustment with PID control.
+		int16_t change = _KP * error + _KI * _integral + _KD * _derivative;
+		change = change / 10;
+		// Change motor speed.
+		int16_t leftSpeed = constrain((_robotSpeedPct + change), -100, 100);
+		int16_t rightSpeed = constrain((_robotSpeedPct - change), -100, 100);
+		motorsWritePct(leftSpeed, rightSpeed);
+	} else {
+		// The robot discovered a perpendicular black line. Stop motors.
 		motorsStop();
-
-		//y detenemos la ejecucion del programa
-		//while(true);
+		// Stop the execution of the control board program.
 		reportActionDone();
-		//setMode(MODE_SIMPLE);
 	}
 }
 
-
 void LineFollow::doCalibration(int speedPct, unsigned int time) {
 	motorsWritePct(speedPct, -speedPct);
 	unsigned long beginTime = millis();
-	while ((millis() - beginTime)<time)
-		ajusta_niveles();
+	while ((millis() - beginTime) < time);
+	_tStartMillis = millis();
 	motorsStop();
 }
-void LineFollow::ajusta_niveles()
-{
-	int lectura = 0;
-
-	for (int count = 0; count<5; count++) {
-		lectura = _IRread(count);
-
-		if (lectura > sensor_blanco[count])
-			sensor_blanco[count] = lectura;
-
-		if (lectura < sensor_negro[count])
-			sensor_negro[count] = lectura;
-	}
-}
-
-
-
-
-

avr/libraries/LottieLemon/src/utility/LottieLemonLineFollow.h

@@ -27,30 +27,36 @@ namespace LottieLemon {
 	public:
 		LineFollow();
 
-		void calibIRs();
 		void runLineFollow();
-		void config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime);
-
+		void config(uint8_t KP, uint8_t KD,
+			uint8_t robotSpeedPct, uint8_t integrationTimeMillis
+		);
+		void config(uint8_t KP, uint8_t KI, uint8_t KD,
+			uint8_t robotSpeedPct, uint8_t integrationTimeMillis
+		);
 		//These are all pure virtual functions, pure VF needs pure specifier "=0"
 		//virtual void motorsWrite(int speedL, int speedR)=0;
 		virtual void motorsWritePct(int speedLpct, int speedRpct) = 0;
 		virtual void motorsStop() = 0;
-		virtual int _IRread(uint8_t num) = 0;
 	protected:
+		virtual int _IRread(uint8_t num) = 0;
 		virtual	void reportActionDone() = 0;
+		void calibIRs();
 
 	private:
 		void doCalibration(int speedPct, unsigned int time);
-		void ajusta_niveles();
-
-		uint8_t KP;
-		uint8_t KD;
-		uint8_t robotSpeed; //percentage
-		uint8_t intergrationTime;
 
-		int lectura_sensor[5], last_error, acu;
-		int sensor_blanco[5];
-		int sensor_negro[5];
+		uint8_t _KP;
+		uint8_t _KI;
+		uint8_t _KD;
+		uint8_t _robotSpeedPct;
+		uint8_t _integrationTimeMillis;
+		unsigned long _tStartMillis;
+
+		uint16_t _sensors[5];
+		int16_t _integral = 0;
+		int16_t _derivative = 0;
+		int16_t _lastError = 0;
 	};
 }