|
@@ -1,11 +1,12 @@
|
1
|
|
-//Verfassser: Felix Stange MET2016
|
2
|
|
-//Datum: 19.07.2017 erstellt, 03.08.2017 zuletzt geändert
|
|
1
|
+//Verfassser: Felix Stange 4056379 MET2016
|
|
2
|
+//Datum: 19.07.2017 erstellt, 11.10.2017 zuletzt geändert
|
3
|
3
|
//Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der
|
4
|
4
|
//Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
5
|
5
|
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es
|
6
|
6
|
//dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren (LSM303) erkannt.
|
7
|
7
|
//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G)
|
8
|
|
-//genutzt. Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die
|
|
8
|
+//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden.
|
|
9
|
+//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die
|
9
|
10
|
//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth).
|
10
|
11
|
//das LCD kann bei Bluetoothkommunikation nicht genutzt werden.
|
11
|
12
|
|
|
@@ -16,6 +17,7 @@ Zumo32U4ProximitySensors proxSensors; //Abstandssensoren
|
16
|
17
|
Zumo32U4LineSensors lineSensors; //Liniensensoren
|
17
|
18
|
Zumo32U4Motors motors;
|
18
|
19
|
Zumo32U4ButtonA buttonA;
|
|
20
|
+Zumo32U4Encoders encoders;
|
19
|
21
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
20
|
22
|
L3G gyro; //Drehbewegungssensor z-Achse
|
21
|
23
|
|
|
@@ -24,17 +26,27 @@ uint16_t lineOffset[3];
|
24
|
26
|
|
25
|
27
|
uint8_t proxValues[4]; //von links (0) nach rechts (3)
|
26
|
28
|
|
27
|
|
-uint32_t turnAngle; //Drehwinkel in °
|
|
29
|
+int16_t countsLeft; //Encoder
|
|
30
|
+int16_t countsRight;
|
|
31
|
+
|
|
32
|
+uint16_t turnAngle = 0; //Drehwinkel
|
28
|
33
|
int16_t turnRate;
|
29
|
34
|
int16_t gyroOffset;
|
30
|
35
|
uint16_t gyroLastUpdate;
|
31
|
36
|
|
32
|
|
-int16_t compassOffset;
|
|
37
|
+uint16_t moveDistance = 0; //Beschleunigung
|
|
38
|
+int16_t moveRate;
|
|
39
|
+int16_t compassOffset;
|
|
40
|
+uint16_t compassLastUpdate;
|
|
41
|
+
|
|
42
|
+uint8_t randy; //Zufallszahl für Richtung
|
33
|
43
|
|
34
|
44
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
35
|
|
-char dir; //Fahrtrichtung, Ereignis
|
|
45
|
+char dir = '0'; //Fahrtrichtung, Ereignis
|
36
|
46
|
char report[120]; //Ausgabe
|
37
|
47
|
|
|
48
|
+/*-----------------------------------------------------------------*/
|
|
49
|
+
|
38
|
50
|
//Setup Liniensensoren
|
39
|
51
|
void LineSensorSetup()
|
40
|
52
|
{
|
|
@@ -115,11 +127,12 @@ void setup()
|
115
|
127
|
|
116
|
128
|
//Kalibrierung der Sensoren
|
117
|
129
|
Serial1.println("sensor calibration");
|
118
|
|
- buttonA.waitForButton();
|
|
130
|
+ buttonA.waitForButton();
|
119
|
131
|
LineSensorSetup();
|
120
|
132
|
TurnSensorSetup();
|
121
|
|
- MoveSensorSetup();
|
122
|
133
|
gyroLastUpdate = micros();
|
|
134
|
+ MoveSensorSetup();
|
|
135
|
+ compassLastUpdate = micros();
|
123
|
136
|
|
124
|
137
|
//Zumo bereit zu starten
|
125
|
138
|
Serial1.println("Zumo ready to start");
|
|
@@ -128,6 +141,49 @@ void setup()
|
128
|
141
|
delay(500);
|
129
|
142
|
}
|
130
|
143
|
|
|
144
|
+/*-----------------------------------------------------------------*/
|
|
145
|
+
|
|
146
|
+//Update Drehbewegungssensor
|
|
147
|
+void TurnSensorUpdate()
|
|
148
|
+{
|
|
149
|
+ gyro.read();
|
|
150
|
+ turnRate = gyro.g.z - gyroOffset;
|
|
151
|
+ uint16_t m = micros();
|
|
152
|
+ uint16_t dt = m - gyroLastUpdate;
|
|
153
|
+ gyroLastUpdate = m;
|
|
154
|
+ int32_t d = (int32_t)turnRate * dt;
|
|
155
|
+ turnAngle += (int64_t)d * 14680064 / 17578125;
|
|
156
|
+ turnAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
|
|
157
|
+}
|
|
158
|
+
|
|
159
|
+// Update Beschleunigungssensor
|
|
160
|
+void MoveSensorUpdate()
|
|
161
|
+{
|
|
162
|
+ compass.read();
|
|
163
|
+ moveRate = compass.a.x - compassOffset;
|
|
164
|
+ uint16_t m = micros();
|
|
165
|
+ uint16_t dt = m - compassLastUpdate;
|
|
166
|
+ compassLastUpdate = m;
|
|
167
|
+ int32_t d = (int32_t)moveRate * dt;
|
|
168
|
+ moveDistance += (int64_t)d * dt / 16384;
|
|
169
|
+ moveDistance = (moveDistance * 1000) / 9,81;
|
|
170
|
+}
|
|
171
|
+
|
|
172
|
+//Update Encoder
|
|
173
|
+//12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
174
|
+void EncoderUpdate()
|
|
175
|
+{
|
|
176
|
+ static uint8_t lastDisplayTime;
|
|
177
|
+ if ((uint8_t)(millis() - lastDisplayTime) >= 100)
|
|
178
|
+ {
|
|
179
|
+ lastDisplayTime = millis();
|
|
180
|
+ countsLeft = encoders.getCountsLeft();
|
|
181
|
+ countsRight = encoders.getCountsRight();
|
|
182
|
+ }
|
|
183
|
+}
|
|
184
|
+
|
|
185
|
+/*-----------------------------------------------------------------*/
|
|
186
|
+
|
131
|
187
|
//Funktion Kollisionserkennung
|
132
|
188
|
void CollisionDetection()
|
133
|
189
|
{
|
|
@@ -201,8 +257,6 @@ void ObstacleAvoidance()
|
201
|
257
|
{
|
202
|
258
|
TurnSensorUpdate();
|
203
|
259
|
}
|
204
|
|
-
|
205
|
|
- ledYellow(0);
|
206
|
260
|
}
|
207
|
261
|
|
208
|
262
|
//Funktion Spurhalten
|
|
@@ -210,7 +264,7 @@ void TrackKeeper()
|
210
|
264
|
{
|
211
|
265
|
|
212
|
266
|
//linke Linie erreicht, nach rechts fahren
|
213
|
|
- if(lineValues[0] < (lineOffset[0] - 200))
|
|
267
|
+ if(lineValues[0] > (lineOffset[0] + 200))
|
214
|
268
|
{
|
215
|
269
|
motors.setSpeeds(maxSpeed, 0);
|
216
|
270
|
ledYellow(1);
|
|
@@ -219,7 +273,7 @@ void TrackKeeper()
|
219
|
273
|
}
|
220
|
274
|
|
221
|
275
|
//rechte Linie erreicht, nach links fahren
|
222
|
|
- else if(lineValues[2] < (lineOffset[2] - 200))
|
|
276
|
+ else if(lineValues[2] > (lineOffset[2] + 200))
|
223
|
277
|
{
|
224
|
278
|
motors.setSpeeds(0, maxSpeed);
|
225
|
279
|
ledYellow(1);
|
|
@@ -228,7 +282,7 @@ void TrackKeeper()
|
228
|
282
|
}
|
229
|
283
|
|
230
|
284
|
//Linie überfahren, zurücksetzen
|
231
|
|
- else if(lineValues[1] < (lineOffset[1] - 100))
|
|
285
|
+ else if(lineValues[1] > (lineOffset[1] + 200))
|
232
|
286
|
{
|
233
|
287
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
234
|
288
|
ledRed(1);
|
|
@@ -246,22 +300,56 @@ void TrackKeeper()
|
246
|
300
|
}
|
247
|
301
|
}
|
248
|
302
|
|
249
|
|
-//Funktion Drehbewegungssensor
|
250
|
|
-void TurnSensorUpdate()
|
251
|
|
-{
|
252
|
|
- gyro.read();
|
253
|
|
- turnRate = gyro.g.z - gyroOffset;
|
254
|
|
- uint16_t m = micros();
|
255
|
|
- uint16_t dt = m - gyroLastUpdate;
|
256
|
|
- gyroLastUpdate = m;
|
257
|
|
- int32_t d = (int32_t)turnRate * dt;
|
258
|
|
- turnAngle += (int64_t)d * 14680064 / 17578125;
|
259
|
|
-}
|
260
|
|
-
|
261
|
303
|
//Funktion Abbiegen
|
262
|
304
|
void Crossroad()
|
263
|
305
|
{
|
|
306
|
+ ledYellow(1);
|
|
307
|
+ dir = 'A';
|
|
308
|
+
|
|
309
|
+ //Zufallszahl erzeugen
|
|
310
|
+ randy = random(3);
|
264
|
311
|
|
|
312
|
+ //Kodierung analysieren
|
|
313
|
+
|
|
314
|
+
|
|
315
|
+ //links Abbiegen
|
|
316
|
+ if(randy == 1)
|
|
317
|
+ {
|
|
318
|
+ //zur Kreuzungsmitte fahren
|
|
319
|
+ while((countsLeft != 200) && (countsRight != 200))
|
|
320
|
+ {
|
|
321
|
+ EncoderUpdate();
|
|
322
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
323
|
+ }
|
|
324
|
+
|
|
325
|
+ //links drehen
|
|
326
|
+ turnAngle = 0;
|
|
327
|
+ TurnSensorUpdate();
|
|
328
|
+ while(turnAngle != 90)
|
|
329
|
+ {
|
|
330
|
+ motors.setSpeeds(0, maxSpeed);
|
|
331
|
+ TurnSensorUpdate();
|
|
332
|
+ }
|
|
333
|
+
|
|
334
|
+ //geradeaus fahren
|
|
335
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
336
|
+ }
|
|
337
|
+
|
|
338
|
+ //rechts Abbiegen
|
|
339
|
+ else if(randy == 2)
|
|
340
|
+ {
|
|
341
|
+ //rechts drehen
|
|
342
|
+ turnAngle = 0;
|
|
343
|
+ TurnSensorUpdate();
|
|
344
|
+ while(turnAngle != 90)
|
|
345
|
+ {
|
|
346
|
+ motors.setSpeeds(maxSpeed, 0);
|
|
347
|
+ TurnSensorUpdate();
|
|
348
|
+ }
|
|
349
|
+
|
|
350
|
+ //geradeaus fahren
|
|
351
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
352
|
+ }
|
265
|
353
|
}
|
266
|
354
|
|
267
|
355
|
//Funktion Serielle Ausgabe
|
|
@@ -271,10 +359,12 @@ void SerialOutput()
|
271
|
359
|
PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Beschleunigung: %6d Winkel: %6d"),
|
272
|
360
|
proxValues[0], proxValues[1], proxValues[2], proxValues[3],
|
273
|
361
|
lineValues[0], lineValues[1], lineValues[2],
|
274
|
|
- dir, compass.a.x, gyro.g.z);
|
|
362
|
+ dir, moveDistance, turnAngle);
|
275
|
363
|
Serial1.println(report);
|
276
|
364
|
}
|
277
|
365
|
|
|
366
|
+/*-----------------------------------------------------------------*/
|
|
367
|
+
|
278
|
368
|
void loop()
|
279
|
369
|
{
|
280
|
370
|
ledGreen(0);
|
|
@@ -288,14 +378,15 @@ void loop()
|
288
|
378
|
proxValues[1] = proxSensors.countsFrontWithLeftLeds();
|
289
|
379
|
proxValues[2] = proxSensors.countsFrontWithRightLeds();
|
290
|
380
|
proxValues[3] = proxSensors.countsRightWithRightLeds();
|
291
|
|
- compass.read();
|
292
|
|
- gyro.read();
|
|
381
|
+ TurnSensorUpdate();
|
|
382
|
+ MoveSensorUpdate();
|
293
|
383
|
|
294
|
384
|
//Funktionsauswahl
|
295
|
|
- if((compass.a.x - compassOffset) > 16000) CollisionDetection();
|
|
385
|
+/* if((compass.a.x - compassOffset) > 4000) CollisionDetection();
|
296
|
386
|
else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
|
297
|
|
- else if (lineValue[0] && lineValue[1] && lineValue[2]) Crossroad();
|
298
|
|
- else TrackKeeper();
|
|
387
|
+ else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200))
|
|
388
|
+ && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
|
|
389
|
+ else TrackKeeper(); */
|
299
|
390
|
|
300
|
391
|
//Ausgabe über Bluetoothverbindung
|
301
|
392
|
SerialOutput();
|