-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsensores3.ino
46 lines (38 loc) · 2.49 KB
/
sensores3.ino
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
//-------------------------------------------------------------
/*
Senssores array valores de rro
0 0 0 0 1 4
0 0 0 1 1 3
0 0 0 1 0 2
0 0 1 1 0 1
0 0 1 0 0 0
0 1 1 0 0 -1
0 1 0 0 0 -2
1 1 0 0 0 -3
1 0 0 0 0 -4
Os estados lógicos possíveis com oconjunto
de 5 sensores com essas distâncias
formando um array estão descritos na tabela
a cima. "pdf disponibilizado"
1 1 1 1 1 0 Robô encontrou linha contínua : STOPPED
0 0 0 0 0 0 Robô não encontrou linha: gire 180o
*/
void readLFSsensors()
{
LFSensor[0] = digitalRead(lineFollowSensor0);
LFSensor[1] = digitalRead(lineFollowSensor1);
LFSensor[2] = digitalRead(lineFollowSensor2);
LFSensor[3] = digitalRead(lineFollowSensor3);
LFSensor[4] = digitalRead(lineFollowSensor4);
if(( LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 4;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 3;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 2;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 1;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 0;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error =- 1;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -2;}
else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -3;}
else if((LFSensor[0]== 1 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -4;}
else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = STOPPED; error = 0;}
else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = NO_LINE; error = 0;}
} // "equacao" pega dos site disponibilizado.