forked from Aaryansh7/IARC_Engelberger
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathread_irsensor
94 lines (71 loc) · 3.86 KB
/
read_irsensor
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
int ir[6];
void setup()
{
Serial.begin(9600);
}
void loop()
{
ir [0] = analogRead(A5);
ir [1] = analogRead(A4);
ir [2] = analogRead(A3);
ir [3] = analogRead(A2);
ir [4] = analogRead(A1);
ir [5] = analogRead(A0);
// straight forward ** WWBBWW WWBWWW WWWBWW
if ( (ir [0] <500 && ir [1] <500 && ir [2] >500 && ir [3] >500 && ir [4] <500 && ir [5] <500) ||
(ir [0] <500 && ir [1] <500 && ir [2] >500 && ir [3] <500 && ir [4] <500 && ir [5] <500) ||
(ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] >500 && ir [4] <500 && ir [5] <500) )
{
Serial.println("forward");
}
//slight left moving forward ** WBBWWW WBWWWW
else if ( (ir [0] <500 && ir [1] >500 && ir [2] >500 && ir [3] <500 && ir [4] <500 && ir [5] <500) ||
(ir [0] <500 && ir [1] >500 && ir [2] <500 && ir [3] <500 && ir [4] <500 && ir [5] <500) )
{
Serial.println("left moving forward");
}
// **WBBBWW
//more left moving forward ** BWWWWW BBWWWW
else if ( (ir [0] >500 && ir [1] <500 && ir [2] <500 && ir [3] <500 && ir [4] <500 && ir [5] <500) ||
(ir [0] >500 && ir [1] >500 && ir [2] <500 && ir [3] <500 && ir [4] <500 && ir [5] <500) )
{
Serial.println("more left moving forward");
}
//slight right moving forward ** WWWBBW WWWWBW
else if ( (ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] >500 && ir [4] >500 && ir [5] <500) ||
(ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] <500 && ir [4] >500 && ir [5] <500) )
{
Serial.println("slight right moving forward");
}
//**WWBBBW
//more right moving forward ** WWWWWB WWWWBB
else if ( (ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] <500 && ir [4] <500 && ir [5] >500) ||
(ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] <500 && ir [4] >500 && ir [5] >500) )
{
Serial.println("more right moving forward");
}
//turn left ** BBBWWW BBBBWW BBBBBW BBBBBB
else if ( (ir [0] >500 && ir [1] >500 && ir [2] >500 && ir [3] <500 && ir [4] <500 && ir [5] <500) ||
(ir [0] >500 && ir [1] >500 && ir [2] >500 && ir [3] >500 && ir [4] <500 && ir [5] <500) ||
(ir [0] >500 && ir [1] >500 && ir [2] >500 && ir [3] >500 && ir [4] >500 && ir [5] <500) ||
(ir [0] >500 && ir [1] >500 && ir [2] >500 && ir [3] >500 && ir [4] >500 && ir [5] >500) )
{
Serial.println("turn left");
}
//turn right -- but LEFT HAND RULE says no right turn, so bot goes straight ** WWWBBB WWBBBB WBBBBB
else if ( (ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] >500 && ir [4] >500 && ir [5] >500) ||
(ir [0] <500 && ir [1] <500 && ir [2] >500 && ir [3] >500 && ir [4] >500 && ir [5] >500) ||
(ir [0] <500 && ir [1] >500 && ir [2] >500 && ir [3] >500 && ir [4] >500 && ir [5] >500) )
{
Serial.println("forward as per left hand rule");
//for right turn only **WWWWWW
if ( (ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] <500 && ir [4] <500 && ir [5] <500) )
Serial.println("only right turn possible");
}
// LEFT HAND RULE -- turning left ** WWWWWW // WWWWWW ONLY IF NODAL LINE=0 INCLUDE LATERRR
else if ( (ir [0] <500 && ir [1] <500 && ir [2] <500 && ir [3] <500 && ir [4] <500 && ir [5] <500) ||
(ir [0] >500 && ir [1] >500 && ir [2] >500 && ir [3] >500 && ir [4] >500 && ir [5] >500) )
{
Serial.println("left");
}
}