-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathline_follower_2.ino
114 lines (96 loc) · 1.71 KB
/
line_follower_2.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
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
// line follower
int mot1=9;
int mot2=6;
int mot3=5;
int mot4=3;
int left=13;
int right=12;
int Left=0;
int Right=0;
void LEFT (void);
void RIGHT (void);
void STOP (void);
void setup()
{
pinMode(mot1,OUTPUT);
pinMode(mot2,OUTPUT);
pinMode(mot3,OUTPUT);
pinMode(mot4,OUTPUT);
pinMode(left,INPUT);
pinMode(right,INPUT);
digitalWrite(left,HIGH);
digitalWrite(right,HIGH);
}
void loop()
{
analogWrite(mot1,255);
analogWrite(mot2,0);
analogWrite(mot3,255);
analogWrite(mot4,0);
while(1)
{
Left=digitalRead(left);
Right=digitalRead(right);
if((Left==0 && Right==1)==1)
LEFT();
else if((Right==0 && Left==1)==1)
RIGHT();
}
}
void LEFT (void)
{
analogWrite(mot3,0);
analogWrite(mot4,30);
while(Left==0)
{
Left=digitalRead(left);
Right=digitalRead(right);
if(Right==0)
{
int lprev=Left;
int rprev=Right;
STOP();
while(((lprev==Left)&&(rprev==Right))==1)
{
Left=digitalRead(left);
Right=digitalRead(right);
}
}
analogWrite(mot1,255);
analogWrite(mot2,0);
}
analogWrite(mot3,255);
analogWrite(mot4,0);
}
void RIGHT (void)
{
analogWrite(mot1,0);
analogWrite(mot2,30);
while(Right==0)
{
Left=digitalRead(left);
Right=digitalRead(right);
if(Left==0)
{
int lprev=Left;
int rprev=Right;
STOP();
while(((lprev==Left)&&(rprev==Right))==1)
{
Left=digitalRead(left);
Right=digitalRead(right);
}
}
analogWrite(mot3,255);
analogWrite(mot4,0);
}
analogWrite(mot1,255);
analogWrite(mot2,0);
}
void STOP (void)
{
analogWrite(mot1,0);
analogWrite(mot2,0);
analogWrite(mot3,0);
analogWrite(mot4,0);
}