-
Notifications
You must be signed in to change notification settings - Fork 0
/
LineFollowing2.ino
120 lines (99 loc) · 2.41 KB
/
LineFollowing2.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
115
116
117
118
119
120
#include <Servo.h>
#include <MeetAndroid.h>
Servo servoLeft;
Servo servoRight;
MeetAndroid meetAndroid;
int onboardLed = 13;
int sensorPin = 3;
int sensorPin1 = 6;
int sensorPin2 = 7;
int qti[3];
int n = 0;
int n1 = 0;
int threshold = 20;
long result = 0;
void setup()
{
Serial.begin(9600);
Serial.println("start");
meetAndroid.registerFunction(testEvent, 'A');
tone(4, 3000, 1000);
pinMode(onboardLed, OUTPUT);
digitalWrite(onboardLed, HIGH);
servoLeft.attach(11);
servoRight.attach(10);
}
//float threshold = RCtime(sensorPin)-((RCtime(sensorPin)+RCtime(sensorPin2))/2);
//read & navigate function
void loop()
{
//Serial.println(threshold);
if (RCtime(sensorPin)>= threshold){
qti[0] = 1;}
else{
qti[0]=0;}
if (RCtime(sensorPin1)>= threshold){
qti[1] = 1;}
else{
qti[1]=0;}
if (RCtime(sensorPin2)>= threshold){
qti[2] = 1;}
else{
qti[2]=0;}
n = random(0,500);
n1 = random(0,500);
if(qti[0]==0 && qti[1]==1 && qti[2]==0){
servoLeft.writeMicroseconds(1700 + n);
servoRight.writeMicroseconds(1300 - n1);
}
else if(qti[0]==1 && qti[1]==1 && qti[2]==0){
servoLeft.writeMicroseconds(1700 + n);
servoRight.writeMicroseconds(1500);
}
else if(qti[0]==1 && qti[1]==0 && qti[2]==0){
servoLeft.writeMicroseconds(1700 + n);
servoRight.writeMicroseconds(1700 + n1);
}
else if(qti[0]==0 && qti[1]==1 && qti[2]==1){
servoLeft.writeMicroseconds(1500 + n);
servoRight.writeMicroseconds(1300 - n1);
}
else if(qti[0]==0 && qti[1]==0 && qti[2]==1){
servoLeft.writeMicroseconds(1300 - n);
servoRight.writeMicroseconds(1300 - n1);
}
else if(qti[0]==1 && qti[1]==1 && qti[2]==1){
servoLeft.writeMicroseconds(1500);
servoRight.writeMicroseconds(1500);
}
delayMicroseconds(40);
//servoLeft.writeMicroseconds(1700);
//servoRight.writeMicroseconds(1300);
//Serial.println(qti[0]);
//Serial.println(qti[1]);
//Serial.println(qti[2]);
}
long RCtime(int sensPin){
long result = 0;
pinMode(sensPin, OUTPUT);
digitalWrite(sensPin, HIGH);
delay(1);
pinMode(sensPin, INPUT);
digitalWrite(sensPin, LOW);
while(digitalRead(sensPin)){
result++;
}
return result;
}
void testEvent(byte flag, byte numOfValues)
{
flushLed(300);
flushLed(300);
}
void flushLed(int time)
{
digitalWrite(onboardLed, LOW);
delay(time);
digitalWrite(onboardLed, HIGH);
delay(time);
}