1 | //#include <ArduinoRobotMotorBoard.h>
|
---|
2 | #include "LineFollow.h"
|
---|
3 |
|
---|
4 | //#define KP 19 //0.1 units
|
---|
5 | //#define KD 14
|
---|
6 | //#define ROBOT_SPEED 100 //percentage
|
---|
7 |
|
---|
8 | //#define KP 11
|
---|
9 | //#define KD 5
|
---|
10 | //#define ROBOT_SPEED 50
|
---|
11 |
|
---|
12 | //#define INTEGRATION_TIME 10 //En ms
|
---|
13 |
|
---|
14 | /*uint8_t KP=11;
|
---|
15 | uint8_t KD=5;
|
---|
16 | uint8_t robotSpeed=50; //percentage
|
---|
17 | uint8_t intergrationTime=10;*/
|
---|
18 |
|
---|
19 | #define NIVEL_PARA_LINEA 50
|
---|
20 |
|
---|
21 | /*int lectura_sensor[5], last_error=0, acu=0;
|
---|
22 |
|
---|
23 | //Estos son los arrays que hay que rellenar con los valores de los sensores
|
---|
24 | //de suelo sobre blanco y negro.
|
---|
25 | int sensor_blanco[]={
|
---|
26 | 0,0,0,0,0};
|
---|
27 | int sensor_negro[]={
|
---|
28 | 1023,1023,1023,1023,1023};
|
---|
29 | */
|
---|
30 | //unsigned long time;
|
---|
31 |
|
---|
32 | //void mueve_robot(int vel_izq, int vel_der);
|
---|
33 | //void para_robot();
|
---|
34 | //void doCalibration(int speedPct, int time);
|
---|
35 | //void ajusta_niveles(); //calibrate values
|
---|
36 |
|
---|
37 | LineFollow::LineFollow(){
|
---|
38 | /*KP=11;
|
---|
39 | KD=5;
|
---|
40 | robotSpeed=50; //percentage
|
---|
41 | intergrationTime=10;*/
|
---|
42 | config(11,5,50,10);
|
---|
43 |
|
---|
44 | for(int i=0;i<5;i++){
|
---|
45 | sensor_blanco[i]=0;
|
---|
46 | sensor_negro[i]=1023;
|
---|
47 | }
|
---|
48 | }
|
---|
49 |
|
---|
50 | void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
|
---|
51 | this->KP=KP;
|
---|
52 | this->KD=KD;
|
---|
53 | this->robotSpeed=robotSpeed;
|
---|
54 | this->intergrationTime=intergrationTime;
|
---|
55 | /*Serial.print("LFC: ");
|
---|
56 | Serial.print(KP);
|
---|
57 | Serial.print(' ');
|
---|
58 | Serial.print(KD);
|
---|
59 | Serial.print(' ');
|
---|
60 | Serial.print(robotSpeed);
|
---|
61 | Serial.print(' ');
|
---|
62 | Serial.println(intergrationTime);*/
|
---|
63 |
|
---|
64 | }
|
---|
65 | void LineFollow::calibIRs(){
|
---|
66 | static bool isInited=false;//So only init once
|
---|
67 | if(isInited)return ;
|
---|
68 |
|
---|
69 | delay(1000);
|
---|
70 |
|
---|
71 | doCalibration(30,500);
|
---|
72 | doCalibration(-30,800);
|
---|
73 | doCalibration(30,500);
|
---|
74 |
|
---|
75 | delay(1000);
|
---|
76 | isInited=true;
|
---|
77 | }
|
---|
78 |
|
---|
79 | void LineFollow::runLineFollow(){
|
---|
80 | for(int count=0; count<5; count++)
|
---|
81 | {
|
---|
82 | lectura_sensor[count]=map(_IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
|
---|
83 | acu+=lectura_sensor[count];
|
---|
84 | }
|
---|
85 |
|
---|
86 | //Serial.println(millis());
|
---|
87 | if (acu > NIVEL_PARA_LINEA)
|
---|
88 | {
|
---|
89 | acu/=5;
|
---|
90 |
|
---|
91 | int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;
|
---|
92 |
|
---|
93 | error = constrain(error,-100,100);
|
---|
94 |
|
---|
95 | //Calculamos la correcion de velocidad mediante un filtro PD
|
---|
96 | int vel = (error * KP)/10 + (error-last_error)*KD;
|
---|
97 |
|
---|
98 | last_error = error;
|
---|
99 |
|
---|
100 | //Corregimos la velocidad de avance con el error de salida del filtro PD
|
---|
101 | int motor_left = constrain((robotSpeed + vel),-100,100);
|
---|
102 | int motor_right =constrain((robotSpeed - vel),-100,100);
|
---|
103 |
|
---|
104 | //Movemos el robot
|
---|
105 | //motorsWritePct(motor_left,motor_right);
|
---|
106 | motorsWritePct(motor_left,motor_right);
|
---|
107 |
|
---|
108 | //Esperamos un poquito a que el robot reaccione
|
---|
109 | delay(intergrationTime);
|
---|
110 | }
|
---|
111 | else
|
---|
112 | {
|
---|
113 | //Hemos encontrado una linea negra
|
---|
114 | //perpendicular a nuestro camino
|
---|
115 | //paramos el robot
|
---|
116 | motorsStop();
|
---|
117 |
|
---|
118 | //y detenemos la ejecución del programa
|
---|
119 | //while(true);
|
---|
120 | reportActionDone();
|
---|
121 | //setMode(MODE_SIMPLE);
|
---|
122 | }
|
---|
123 | }
|
---|
124 |
|
---|
125 |
|
---|
126 | void LineFollow::doCalibration(int speedPct, int time){
|
---|
127 | motorsWritePct(speedPct, -speedPct);
|
---|
128 | unsigned long beginTime = millis();
|
---|
129 | while((millis()-beginTime)<time)
|
---|
130 | ajusta_niveles();
|
---|
131 | motorsStop();
|
---|
132 | }
|
---|
133 | void LineFollow::ajusta_niveles()
|
---|
134 | {
|
---|
135 | int lectura=0;
|
---|
136 |
|
---|
137 | for(int count=0; count<5; count++){
|
---|
138 | lectura=_IRread(count);
|
---|
139 |
|
---|
140 | if (lectura > sensor_blanco[count])
|
---|
141 | sensor_blanco[count]=lectura;
|
---|
142 |
|
---|
143 | if (lectura < sensor_negro[count])
|
---|
144 | sensor_negro[count]=lectura;
|
---|
145 | }
|
---|
146 | }
|
---|
147 |
|
---|
148 |
|
---|
149 |
|
---|
150 |
|
---|
151 |
|
---|
152 |
|
---|