-
Notifications
You must be signed in to change notification settings - Fork 3
/
LineFollowerV1.ino
204 lines (158 loc) · 6.63 KB
/
LineFollowerV1.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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
/*
LineFollower V1, ejemplo sencillo de seguidor de lineas. Tambien se pueden ver el uso de la libreria Pussbutton y ZumoBuzzer.
Cualquier comentarios o duda, puedes escribirme a [email protected]
@_JuraseC
*/
#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <ZumoBuzzer.h>
#include <ZumoMotors.h>
#include <Pushbutton.h>
// Motor speed when driving straight. SPEED should always
// have a positive value, otherwise the Zumo will travel in the
// wrong direction.
#define SPEED 400
// Define an array for holding sensor values.
#define NUM_SENSORS 6
//#define HIGH_SPEED 150
// SENSOR_THRESHOLD is a value to compare reflectance sensor
// readings to to decide if the sensor is over a black line
#define SENSOR_THRESHOLD 300
// Motor speed when turning. TURN_SPEED should always
// have a positive value, otherwise the Zumo will turn
// in the wrong direction.
#define TURN_SPEED 200
// ABOVE_LINE is a helper macro that takes returns
// 1 if the sensor is over the line and 0 if otherwise
#define ABOVE_LINE(sensor)((sensor) > SENSOR_THRESHOLD)
#define MELODY_LENGTH 95
ZumoMotors motors;
ZumoBuzzer buzzer;
Pushbutton button(ZUMO_BUTTON);
ZumoReflectanceSensorArray reflectanceSensors;
/*const char fugue[] PROGMEM =
"! O5 L16 agafaea dac+adaea fa<aa<bac#a dac#adaea f"
"O6 dcd<b-d<ad<g d<f+d<gd<ad<b- d<dd<ed<f+d<g d<f+d<gd<ad"
"L8 MS <b-d<b-d MLe-<ge-<g MSc<ac<a ML d<fd<f O5 MS b-gb-g"
"ML >c#e>c#e MS afaf ML gc#gc# MS fdfd ML e<b-e<b-"
"O6 L16ragafaea dac#adaea fa<aa<bac#a dac#adaea faeadaca"
"<b-acadg<b-g egdgcg<b-g <ag<b-gcf<af dfcf<b-f<af"
"<gf<af<b-e<ge c#e<b-e<ae<ge <fe<ge<ad<fd"
"O5 e>ee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d"
"e>d>c#>db>d>c#b >c#agaegfe f O6 dc#dfdc#<b c#4";*/
// These arrays take up a total of 285 bytes of RAM (out of a limit of 1k (ATmega168), 2k (ATmega328), or 2.5k(ATmega32U4))
unsigned char note[MELODY_LENGTH] =
{
NOTE_E(5), SILENT_NOTE, NOTE_E(5), SILENT_NOTE, NOTE_E(5), SILENT_NOTE, NOTE_C(5), NOTE_E(5),
NOTE_G(5), SILENT_NOTE, NOTE_G(4), SILENT_NOTE,
NOTE_C(5), NOTE_G(4), SILENT_NOTE, NOTE_E(4), NOTE_A(4), NOTE_B(4), NOTE_B_FLAT(4), NOTE_A(4), NOTE_G(4),
NOTE_E(5), NOTE_G(5), NOTE_A(5), NOTE_F(5), NOTE_G(5), SILENT_NOTE, NOTE_E(5), NOTE_C(5), NOTE_D(5), NOTE_B(4),
NOTE_C(5), NOTE_G(4), SILENT_NOTE, NOTE_E(4), NOTE_A(4), NOTE_B(4), NOTE_B_FLAT(4), NOTE_A(4), NOTE_G(4),
NOTE_E(5), NOTE_G(5), NOTE_A(5), NOTE_F(5), NOTE_G(5), SILENT_NOTE, NOTE_E(5), NOTE_C(5), NOTE_D(5), NOTE_B(4),
SILENT_NOTE, NOTE_G(5), NOTE_F_SHARP(5), NOTE_F(5), NOTE_D_SHARP(5), NOTE_E(5), SILENT_NOTE,
NOTE_G_SHARP(4), NOTE_A(4), NOTE_C(5), SILENT_NOTE, NOTE_A(4), NOTE_C(5), NOTE_D(5),
SILENT_NOTE, NOTE_G(5), NOTE_F_SHARP(5), NOTE_F(5), NOTE_D_SHARP(5), NOTE_E(5), SILENT_NOTE,
NOTE_C(6), SILENT_NOTE, NOTE_C(6), SILENT_NOTE, NOTE_C(6),
SILENT_NOTE, NOTE_G(5), NOTE_F_SHARP(5), NOTE_F(5), NOTE_D_SHARP(5), NOTE_E(5), SILENT_NOTE,
NOTE_G_SHARP(4), NOTE_A(4), NOTE_C(5), SILENT_NOTE, NOTE_A(4), NOTE_C(5), NOTE_D(5),
SILENT_NOTE, NOTE_E_FLAT(5), SILENT_NOTE, NOTE_D(5), NOTE_C(5)
};
unsigned int duration[MELODY_LENGTH] =
{
100, 25, 125, 125, 125, 125, 125, 250, 250, 250, 250, 250,
375, 125, 250, 375, 250, 250, 125, 250, 167, 167, 167, 250, 125, 125,
125, 250, 125, 125, 375,
375, 125, 250, 375, 250, 250, 125, 250, 167, 167, 167, 250, 125, 125,
125, 250, 125, 125, 375,
250, 125, 125, 125, 250, 125, 125, 125, 125, 125, 125, 125, 125, 125,
250, 125, 125, 125, 250, 125, 125, 200, 50, 100, 25, 500,
250, 125, 125, 125, 250, 125, 125, 125, 125, 125, 125, 125, 125, 125,
250, 250, 125, 375, 500
};
unsigned int sensorValues[NUM_SENSORS];
unsigned int last_proportional=0;
unsigned int position=0;
int power_difference=0;
unsigned char currentIdx=0;
unsigned int sensors[NUM_SENSORS];
int offset_from_center;
//int power_difference;
void setup(){
//unsigned int sensors[NUM_SENSORS];
unsigned short count = 0;
unsigned short last_status = 0;
int turn_direction = 1;
reflectanceSensors.init();
delay(500);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH); // turn on LED to indicate we are in calibration mode
button.waitForButton();
runCalibrate();
}
void loop(){
logica();
}
void logica(){
while(1){
// Get the position of the line.
position = reflectanceSensors.readLine(sensors);
// The offset_from_center should be 0 when we are on the line.
offset_from_center = ((int)position) - 2500;
// Compute the difference between the two motor power settings,
// m1 - m2. If this is a positive number the robot will turn
// to the left. If it is a negative number, the robot will
// turn to the right, and the magnitude of the number determines
// the sharpness of the turn.
power_difference = offset_from_center / 3;
// Compute the actual motor settings. We never set either motor
// to a negative value.
if(power_difference > SPEED)
power_difference = SPEED;
if(power_difference < -SPEED)
power_difference = -SPEED;
if(power_difference < 0)
motors.setSpeeds(SPEED + power_difference, SPEED);
else
motors.setSpeeds(SPEED, SPEED - power_difference);
if (currentIdx < MELODY_LENGTH && !buzzer.isPlaying()){
// play note at max volume
buzzer.playNote(note[currentIdx], duration[currentIdx], 15);
currentIdx++;
}
if(!buzzer.isPlaying())
currentIdx=0;
}
}
void runCalibrate(){
unsigned short count = 0;
unsigned short last_status = 0;
int turn_direction = 1;
// Calibrate the Zumo by sweeping it from left to right
for(int i = 0; i < 4; i ++){
// Zumo will turn clockwise if turn_direction = 1.
// If turn_direction = -1 Zumo will turn counter-clockwise.
turn_direction *= -1;
// Turn direction.
motors.setSpeeds(turn_direction * TURN_SPEED, -1*turn_direction * TURN_SPEED);
// This while loop monitors line position
// until the turn is complete.
while(count < 2){
reflectanceSensors.calibrate();
reflectanceSensors.readLine(sensors);
if(turn_direction < 0){
// If the right most sensor changes from (over white space -> over
// line or over line -> over white space) add 1 to count.
count += ABOVE_LINE(sensors[5]) ^ last_status;
last_status = ABOVE_LINE(sensors[5]);
}
else{
// If the left most sensor changes from (over white space -> over
// line or over line -> over white space) add 1 to count.
count += ABOVE_LINE(sensors[0]) ^ last_status;
last_status = ABOVE_LINE(sensors[0]);
}
}
count = 0;
last_status = 0;
}
}