-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathCodigo siguelineas
214 lines (189 loc) · 6.31 KB
/
Codigo siguelineas
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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
//PIECE OF CAKE
//CÓDIGO PARA SIGUELINEAS
#include <QTRSensors.h>
#define NUM_SENSORS 8 // numero de sensores usados
#define TIMEOUT 2500 // esperar 2.5 ms para tener una respuesta del sensado
#define EMITTER_PIN 6 // este pin controla el led on del los sensores (enciende y apaga)
// Conexiones
int pwma = 3;
int pwmb = 5;
int ai1 = 2;
int ai2 = 4;
int bi1 = 7;
int bi2 = 8;
int sensorfrontal=10;
int interruptor = 9; //Interruptor para comprobar el carril
int carril;
QTRSensorsRC qtrrc((unsigned char[]) {19, 18, 17, 16, 15, 14, 11, 12},
NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void pieceofcake(int option);
void obstaculo();
// Inicialización
void setup() {
pinMode(pwma,OUTPUT);
pinMode(ai1,OUTPUT);
pinMode(ai2,OUTPUT);
pinMode(pwmb,OUTPUT);
pinMode(bi1,OUTPUT);
pinMode(bi2,OUTPUT);
pinMode(interruptor, INPUT_PULLUP);
carril=digitalRead(interruptor);
//Carril = 1 -> Izquierda
//Carril = 0 -> Derecha
attachInterrupt(digitalPinToInterrupt(sensorfrontal), obstaculo, RISING); //Interrupcion para cambiar de carril
delay(500);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH); //este led encendido indica que comienza la calibracion
for (int i = 0; i < 200; i++) // la calibracion se lleva a cabo por 5 segundos
{
qtrrc.calibrate(); // funcion para calibrar los sensores
}
digitalWrite(13, LOW); // se paga el led para que indique que termino la calibracion
// imprime la calibracion minima de los sensores
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// imprime la calibracion maxima de los sensores
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
}
// Rutina
void loop() {
// read calibrated sensor values and obtain a measure of the line position from 0 to 5000
// To get raw sensor values, call:
// qtrrc.read(sensorValues); instead of unsigned int position = qtrrc.readLine(sensorValues);
unsigned int position = qtrrc.readLine(sensorValues);
// print the sensor values as numbers from 0 to 1000, where 0 means maximum reflectance and
// 1000 means minimum reflectance, followed by the line position
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
Serial.print(sensorValues[i]);
Serial.print('\t');
}
Serial.println(); // uncomment this line if you are using raw values
//Serial.println(position); // comment this line out if you are using raw values
if(sensorValues[0]<400 && sensorValues[1]<400 && sensorValues[2]<400 && sensorValues[3]>400 && sensorValues[4]>400 && sensorValues[5]<400 && sensorValues[6]<400 && sensorValues[7]<400){
pieceofcake(1);
}else
if (sensorValues[0]<400 && sensorValues[1]<400 && sensorValues[2]<400 && sensorValues[3]<400 && sensorValues[4]>400 && sensorValues[5]>400 && sensorValues[6]<400 && sensorValues[7]<400){
pieceofcake(2);
}else
if (sensorValues[0]<400 && sensorValues[1]<400 && sensorValues[2]<400 && sensorValues[3]<400 && sensorValues[4]<400 && sensorValues[5]>400 && sensorValues[6]>400 && sensorValues[7]<400){
pieceofcake(3);
}else
if (sensorValues[0]<400 && sensorValues[1]<400 && sensorValues[2]<400 && sensorValues[3]<400 && sensorValues[4]<400 && sensorValues[5]<400 && sensorValues[6]>400 && sensorValues[7]>400){
pieceofcake(4);
}else
if (sensorValues[0]<400 && sensorValues[1]<400 && sensorValues[2]>400 && sensorValues[3]>400 && sensorValues[4]<400 && sensorValues[5]<400 && sensorValues[6]<400 && sensorValues[7]<400){
pieceofcake(5);
}else
if (sensorValues[0]<400 && sensorValues[1]>400 && sensorValues[2]>400 && sensorValues[3]<400 && sensorValues[4]<400 && sensorValues[5]<400 && sensorValues[6]<400 && sensorValues[7]<400){
pieceofcake(6);
}else
if(sensorValues[0]>400 && sensorValues[1]>400 && sensorValues[2]<400 && sensorValues[3]<400 && sensorValues[4]<400 && sensorValues[5]<400 && sensorValues[6]<400 && sensorValues[7]<400){
pieceofcake(7);
}
delay(50);
}
//1. Recto
//2. Izda leve
//3. Izda medio
//4. Izda max
//5. Dcha leve
//6. Dcha medio
//7. Dcha max
void pieceofcake(int option){
switch (option){
case 1:
analogWrite(pwma,255);
analogWrite(pwmb,255);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
break;
case 2:
analogWrite(pwma,125);
analogWrite(pwmb,255);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
break;
case 3:
analogWrite(pwma,50);
analogWrite(pwmb,255);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
break;
case 4:
analogWrite(pwma,50);
analogWrite(pwmb,255);
digitalWrite(ai1,HIGH);
digitalWrite(ai2,LOW);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
break;
case 5:
analogWrite(pwma,255);
analogWrite(pwmb,125);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
break;
case 6:
analogWrite(pwma,255);
analogWrite(pwmb,50);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
break;
case 7:
analogWrite(pwma,255);
analogWrite(pwmb,50);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,LOW);
digitalWrite(bi2,HIGH);
break;
}
}
void obstaculo(){
if (carril==0){
//Mover al carril izquierdo
do{
analogWrite(pwma,150);
analogWrite(pwmb,255);
digitalWrite(ai1,LOW);
digitalWrite(ai2,HIGH);
digitalWrite(bi1,HIGH);
digitalWrite(bi2,LOW);
}while(sensorValues[3]>400 && sensorValues[4]>400);
carril = !carril;
}else if (carril==1){
do{
analogWrite(pwma,255);
analogWrite(pwmb,150);
digitalWrite(ai1,HIGH);
digitalWrite(ai2,LOW);
digitalWrite(bi1,LOW);
digitalWrite(bi2,HIGH);
}while(sensorValues[3]>400 && sensorValues[4]>400);
carril = !carril;
}
}