1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
|
//#include <ArduinoRobotMotorBoard.h>
#include "LineFollow.h"
//#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::calibIRs(){
static bool isInited=false;//So only init once
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);
}
else
{
//Hemos encontrado una linea negra
//perpendicular a nuestro camino
//paramos el robot
motorsStop();
//y detenemos la ejecucin del programa
//while(true);
reportActionDone();
//setMode(MODE_SIMPLE);
}
}
void LineFollow::doCalibration(int speedPct, int time){
motorsWritePct(speedPct, -speedPct);
unsigned long beginTime = millis();
while((millis()-beginTime)<time)
ajusta_niveles();
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;
}
}
|
