-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathlinefollowing.ino
More file actions
139 lines (122 loc) · 6.38 KB
/
linefollowing.ino
File metadata and controls
139 lines (122 loc) · 6.38 KB
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
/*NOT DONE DON'T TOUCH YET*/
/*
// Pin definitions
#define linesensor 1
#define MOTORL 3
#define MOTORR 2
void setup() {
// put your setup code here, to run once:
pinMode(LINESENSOR, INPUT);
pinMode(MOTORL,OUTPUT);
pinMode(MOTORR,OUTPUT);
}
void loop() {
}
*/
//-------SENSOR READING CODE----------
void readLFMSensors()
{
LFMSensor[0] = digitalRead(LFMSensor0);
LFMSensor[1] = digitalRead(LFMSensor1);
LFMSensor[2] = digitalRead(LFMSensor2);
LFMSensor[3] = digitalRead(LFMSensor3);
LFMSensor[4] = digitalRead(LFMSensor4);
LFMSensor[5] = digitalRead(LFMSensor5);
LFMSensor[6] = digitalRead(LFMSensor6);
LFMSensor[7] = digitalRead(LFMSensor7);
if(( ((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 1 )) {mode = FOLLOWING_LINE; error = 7;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 1 )&&(LFMSensor[7]== 1 )) {mode = FOLLOWING_LINE; error = 6;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 1 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = 5;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 1 )&&(LFMSensor[6]== 1 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = 4;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 1 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = 3;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 1 )&&(LFMSensor[5]== 1 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = 2;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 1 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = 1;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 1 )&&(LFMSensor[4]== 1 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error =- 0;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 1 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -1;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 1 )&&(LFMSensor[3]== 1 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -2;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 1 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -3;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 1 )&&(LFMSensor[2]== 1 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -4;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 1 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -5;}
else if((LFMSensor[0]== 1 )&&(LFMSensor[1]== 1 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -6;}
else if((LFMSensor[0]== 1 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 0 )&&(LFMSensor[4]== 0 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = FOLLOWING_LINE; error = -7;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 1 )&&(LFMSensor[4]== 1 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = STOPPED; error = 0;}
else if((LFMSensor[0]== 0 )&&(LFMSensor[1]== 0 )&&(LFMSensor[2]== 0 )&&(LFMSensor[3]== 1 )&&(LFMSensor[4]== 1 )&&(LFMSensor[5]== 0 )&&(LFMSensor[6]== 0 )&&(LFMSensor[7]== 0 )) {mode = NO_LINE; error = 0;}
}
//-------SENSOR TESTING-------------
void testLFMSensors()
{
int LFMS0 = digitalRead(LFMSensor0);
int LFMS1 = digitalRead(LFMSensor1);
int LFMS2 = digitalRead(LFMSensor2);
int LFMS3 = digitalRead(LFMSensor3);
int LFMS4 = digitalRead(LFMSensor4);
int LFMS5 = digitalRead(LFMSensor5);
int LFMS6 = digitalRead(LFMSensor6);
int LFMS7 = digitalRead(LFMSensor7);
Serial.print ("LFMS: L 0 1 2 3 4 5 6 7 R --> ");
Serial.print (LFMS0);
Serial.print (" ");
Serial.print (LFMS1);
Serial.print (" ");
Serial.print (LFMS2);
Serial.print (" ");
Serial.print (LFMS3);
Serial.print (" ");
Serial.print (LFMS54);
Serial.print (" ");
Serial.print (LFMS5);
Serial.print (" ");
Serial.print (LFMS6);
Serial.print (" ");
Serial.print (LFMS7);
Serial.print (" --> ");
Serial.print (" P: ");
Serial.print (P);
Serial.print (" I: ");
Serial.print (I);
Serial.print (" D: ");
Serial.print (D);
Serial.print (" PID: ");
Serial.println (PIDvalue);
}
// SERVO TESTING
#include <Servo.h>
Servo servoLeft; // Define left servo
Servo servoRight; // Define right servo
void setup() {
servoLeft.attach(10); // Set left servo to digital pin 10
servoRight.attach(9); // Set right servo to digital pin 9
}
void loop() { // Loop through motion tests
forward(); // Example: move forward
delay(2000); // Wait 2000 milliseconds (2 seconds)
reverse();
delay(2000);
turnRight();
delay(2000);
turnLeft();
delay(2000);
stopRobot();
delay(2000);
}
// Motion routines for forward, reverse, turns, and stop
void forward() {
servoLeft.write(0);
servoRight.write(180);
}
void reverse() {
servoLeft.write(180);
servoRight.write(0);
}
void turnRight() {
servoLeft.write(180);
servoRight.write(180);
}
void turnLeft() {
servoLeft.write(0);
servoRight.write(0);
}
void stopRobot() {
servoLeft.write(90);
servoRight.write(90);
}