Skip to content

Commit 7d95492

Browse files
j54n1nj54n1n
committed
Added new PID algorithm
1 parent b1b0e1d commit 7d95492

File tree

2 files changed

+64
-134
lines changed

2 files changed

+64
-134
lines changed

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

Lines changed: 45 additions & 121 deletions
Original file line numberDiff line numberDiff line change
@@ -20,151 +20,75 @@
2020
#include "LottieLemonLineFollow.h"
2121
using namespace LottieLemon;
2222

23-
//#define KP 19 //0.1 units
24-
//#define KD 14
25-
//#define ROBOT_SPEED 100 //percentage
26-
27-
//#define KP 11
28-
//#define KD 5
29-
//#define ROBOT_SPEED 50
30-
31-
//#define INTEGRATION_TIME 10 //En ms
32-
33-
/*uint8_t KP=11;
34-
uint8_t KD=5;
35-
uint8_t robotSpeed=50; //percentage
36-
uint8_t intergrationTime=10;*/
37-
38-
#define NIVEL_PARA_LINEA 50
39-
40-
/*int lectura_sensor[5], last_error=0, acu=0;
41-
42-
//Estos son los arrays que hay que rellenar con los valores de los sensores
43-
//de suelo sobre blanco y negro.
44-
int sensor_blanco[]={
45-
0,0,0,0,0};
46-
int sensor_negro[]={
47-
1023,1023,1023,1023,1023};
48-
*/
49-
//unsigned long time;
50-
51-
//void mueve_robot(int vel_izq, int vel_der);
52-
//void para_robot();
53-
//void doCalibration(int speedPct, int time);
54-
//void ajusta_niveles(); //calibrate values
55-
5623
LineFollow::LineFollow() {
57-
/*KP=11;
58-
KD=5;
59-
robotSpeed=50; //percentage
60-
intergrationTime=10;*/
6124
config(11, 5, 50, 10);
62-
63-
for (int i = 0; i<5; i++) {
64-
sensor_blanco[i] = 0;
65-
sensor_negro[i] = 1023;
66-
}
6725
}
6826

69-
void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime) {
70-
this->KP = KP;
71-
this->KD = KD;
72-
this->robotSpeed = robotSpeed;
73-
this->intergrationTime = intergrationTime;
74-
/*Serial.print("LFC: ");
75-
Serial.print(KP);
76-
Serial.print(' ');
77-
Serial.print(KD);
78-
Serial.print(' ');
79-
Serial.print(robotSpeed);
80-
Serial.print(' ');
81-
Serial.println(intergrationTime);*/
27+
void LineFollow::config(uint8_t KP, uint8_t KD,
28+
uint8_t robotSpeedPct, uint8_t integrationTimeMillis) {
29+
config(KP, 0, KD, robotSpeedPct, integrationTimeMillis);
30+
}
8231

32+
void LineFollow::config(uint8_t KP, uint8_t KI, uint8_t KD,
33+
uint8_t robotSpeedPct, uint8_t integrationTimeMillis) {
34+
_KP = KP;
35+
_KI = KI;
36+
_KD = KD;
37+
_robotSpeedPct = robotSpeedPct;
38+
_integrationTimeMillis = integrationTimeMillis;
8339
}
84-
void LineFollow::calibIRs() {
85-
static bool isInited = false;//So only init once
86-
if (isInited)return;
8740

41+
void LineFollow::calibIRs() {
42+
// So only init once.
43+
static bool isInited = false;
44+
if (isInited) {
45+
return;
46+
}
8847
delay(1000);
89-
9048
doCalibration(30, 500);
9149
doCalibration(-30, 800);
9250
doCalibration(30, 500);
93-
9451
delay(1000);
9552
isInited = true;
9653
}
9754

9855
void LineFollow::runLineFollow() {
99-
for (int count = 0; count<5; count++)
100-
{
101-
lectura_sensor[count] = map(_IRread(count), sensor_negro[count], sensor_blanco[count], 0, 127);
102-
acu += lectura_sensor[count];
103-
}
104-
105-
//Serial.println(millis());
106-
if (acu > NIVEL_PARA_LINEA)
107-
{
108-
acu /= 5;
109-
110-
int error = ((lectura_sensor[0] << 6) + (lectura_sensor[1] << 5) - (lectura_sensor[3] << 5) - (lectura_sensor[4] << 6)) / acu;
111-
112-
error = constrain(error, -100, 100);
113-
114-
//Calculamos la correcion de velocidad mediante un filtro PD
115-
int vel = (error * KP) / 10 + (error - last_error)*KD;
116-
117-
last_error = error;
118-
119-
//Corregimos la velocidad de avance con el error de salida del filtro PD
120-
int motor_left = constrain((robotSpeed + vel), -100, 100);
121-
int motor_right = constrain((robotSpeed - vel), -100, 100);
122-
123-
//Movemos el robot
124-
//motorsWritePct(motor_left,motor_right);
125-
motorsWritePct(motor_left, motor_right);
126-
127-
//Esperamos un poquito a que el robot reaccione
128-
delay(intergrationTime);
56+
enum { STOP_LINE_VALUE = 1500 };
57+
uint32_t weight = 0;
58+
uint16_t sum = 0;
59+
for (int i = 0; i < 5; i++) {
60+
_sensors[i] = _IRread(i);
61+
weight += _sensors[i] * (i + 1);
62+
sum += _sensors[i];
12963
}
130-
else
131-
{
132-
//Hemos encontrado una linea negra
133-
//perpendicular a nuestro camino
134-
//paramos el robot
64+
if (sum > STOP_LINE_VALUE) {
65+
if ((millis() - _tStartMillis) <= _integrationTimeMillis) {
66+
return;
67+
}
68+
_tStartMillis = millis();
69+
int16_t error = 100 * weight / sum - 300;
70+
_integral = _integral + error;
71+
_derivative = error - _lastError;
72+
_lastError = error;
73+
// Calculate the adjustment with PID control.
74+
int16_t change = _KP * error + _KI * _integral + _KD * _derivative;
75+
change = change / 10;
76+
// Change motor speed.
77+
int16_t leftSpeed = constrain((_robotSpeedPct + change), -100, 100);
78+
int16_t rightSpeed = constrain((_robotSpeedPct - change), -100, 100);
79+
motorsWritePct(leftSpeed, rightSpeed);
80+
} else {
81+
// The robot discovered a perpendicular black line. Stop motors.
13582
motorsStop();
136-
137-
//y detenemos la ejecucion del programa
138-
//while(true);
83+
// Stop the execution of the control board program.
13984
reportActionDone();
140-
//setMode(MODE_SIMPLE);
14185
}
14286
}
14387

144-
14588
void LineFollow::doCalibration(int speedPct, unsigned int time) {
14689
motorsWritePct(speedPct, -speedPct);
14790
unsigned long beginTime = millis();
148-
while ((millis() - beginTime)<time)
149-
ajusta_niveles();
91+
while ((millis() - beginTime) < time);
92+
_tStartMillis = millis();
15093
motorsStop();
15194
}
152-
void LineFollow::ajusta_niveles()
153-
{
154-
int lectura = 0;
155-
156-
for (int count = 0; count<5; count++) {
157-
lectura = _IRread(count);
158-
159-
if (lectura > sensor_blanco[count])
160-
sensor_blanco[count] = lectura;
161-
162-
if (lectura < sensor_negro[count])
163-
sensor_negro[count] = lectura;
164-
}
165-
}
166-
167-
168-
169-
170-

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

Lines changed: 19 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -27,30 +27,36 @@ namespace LottieLemon {
2727
public:
2828
LineFollow();
2929

30-
void calibIRs();
3130
void runLineFollow();
32-
void config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime);
33-
31+
void config(uint8_t KP, uint8_t KD,
32+
uint8_t robotSpeedPct, uint8_t integrationTimeMillis
33+
);
34+
void config(uint8_t KP, uint8_t KI, uint8_t KD,
35+
uint8_t robotSpeedPct, uint8_t integrationTimeMillis
36+
);
3437
//These are all pure virtual functions, pure VF needs pure specifier "=0"
3538
//virtual void motorsWrite(int speedL, int speedR)=0;
3639
virtual void motorsWritePct(int speedLpct, int speedRpct) = 0;
3740
virtual void motorsStop() = 0;
38-
virtual int _IRread(uint8_t num) = 0;
3941
protected:
42+
virtual int _IRread(uint8_t num) = 0;
4043
virtual void reportActionDone() = 0;
44+
void calibIRs();
4145

4246
private:
4347
void doCalibration(int speedPct, unsigned int time);
44-
void ajusta_niveles();
45-
46-
uint8_t KP;
47-
uint8_t KD;
48-
uint8_t robotSpeed; //percentage
49-
uint8_t intergrationTime;
5048

51-
int lectura_sensor[5], last_error, acu;
52-
int sensor_blanco[5];
53-
int sensor_negro[5];
49+
uint8_t _KP;
50+
uint8_t _KI;
51+
uint8_t _KD;
52+
uint8_t _robotSpeedPct;
53+
uint8_t _integrationTimeMillis;
54+
unsigned long _tStartMillis;
55+
56+
uint16_t _sensors[5];
57+
int16_t _integral = 0;
58+
int16_t _derivative = 0;
59+
int16_t _lastError = 0;
5460
};
5561
}
5662

0 commit comments

Comments
 (0)