|
@@ -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,19 +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
|
|
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;
|
33
|
41
|
|
34
|
|
-uint4_t rand; //Zufallszahl für Richtung
|
|
42
|
+uint8_t randy; //Zufallszahl für Richtung
|
35
|
43
|
|
36
|
44
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
37
|
|
-char dir; //Fahrtrichtung, Ereignis
|
|
45
|
+char dir = '0'; //Fahrtrichtung, Ereignis
|
38
|
46
|
char report[120]; //Ausgabe
|
39
|
47
|
|
|
48
|
+/*-----------------------------------------------------------------*/
|
|
49
|
+
|
40
|
50
|
//Setup Liniensensoren
|
41
|
51
|
void LineSensorSetup()
|
42
|
52
|
{
|
|
@@ -120,8 +130,9 @@ void setup()
|
120
|
130
|
buttonA.waitForButton();
|
121
|
131
|
LineSensorSetup();
|
122
|
132
|
TurnSensorSetup();
|
123
|
|
- MoveSensorSetup();
|
124
|
133
|
gyroLastUpdate = micros();
|
|
134
|
+ MoveSensorSetup();
|
|
135
|
+ compassLastUpdate = micros();
|
125
|
136
|
|
126
|
137
|
//Zumo bereit zu starten
|
127
|
138
|
Serial1.println("Zumo ready to start");
|
|
@@ -130,6 +141,49 @@ void setup()
|
130
|
141
|
delay(500);
|
131
|
142
|
}
|
132
|
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
|
+
|
133
|
187
|
//Funktion Kollisionserkennung
|
134
|
188
|
void CollisionDetection()
|
135
|
189
|
{
|
|
@@ -246,18 +300,6 @@ 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
|
{
|
|
@@ -265,17 +307,20 @@ void Crossroad()
|
265
|
307
|
dir = 'A';
|
266
|
308
|
|
267
|
309
|
//Zufallszahl erzeugen
|
268
|
|
- rand = random(3);
|
|
310
|
+ randy = random(3);
|
269
|
311
|
|
270
|
312
|
//Kodierung analysieren
|
271
|
313
|
|
272
|
314
|
|
273
|
315
|
//links Abbiegen
|
274
|
|
- if(rand == 1)
|
|
316
|
+ if(randy == 1)
|
275
|
317
|
{
|
276
|
318
|
//zur Kreuzungsmitte fahren
|
277
|
|
- while((lineValues[0] < (lineOffset[0] + 200)) || (lineValues[1] < (lineOffset[1] + 200))
|
278
|
|
- || (lineValues[2] < (lineOffset[2] + 200))) motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
319
|
+ while((countsLeft != 200) && (countsRight != 200))
|
|
320
|
+ {
|
|
321
|
+ EncoderUpdate();
|
|
322
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
323
|
+ }
|
279
|
324
|
|
280
|
325
|
//links drehen
|
281
|
326
|
turnAngle = 0;
|
|
@@ -291,7 +336,7 @@ void Crossroad()
|
291
|
336
|
}
|
292
|
337
|
|
293
|
338
|
//rechts Abbiegen
|
294
|
|
- else if(rand == 2)
|
|
339
|
+ else if(randy == 2)
|
295
|
340
|
{
|
296
|
341
|
//rechts drehen
|
297
|
342
|
turnAngle = 0;
|
|
@@ -314,10 +359,12 @@ void SerialOutput()
|
314
|
359
|
PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Beschleunigung: %6d Winkel: %6d"),
|
315
|
360
|
proxValues[0], proxValues[1], proxValues[2], proxValues[3],
|
316
|
361
|
lineValues[0], lineValues[1], lineValues[2],
|
317
|
|
- dir, compass.a.x, gyro.g.z);
|
|
362
|
+ dir, moveDistance, turnAngle);
|
318
|
363
|
Serial1.println(report);
|
319
|
364
|
}
|
320
|
365
|
|
|
366
|
+/*-----------------------------------------------------------------*/
|
|
367
|
+
|
321
|
368
|
void loop()
|
322
|
369
|
{
|
323
|
370
|
ledGreen(0);
|
|
@@ -331,15 +378,15 @@ void loop()
|
331
|
378
|
proxValues[1] = proxSensors.countsFrontWithLeftLeds();
|
332
|
379
|
proxValues[2] = proxSensors.countsFrontWithRightLeds();
|
333
|
380
|
proxValues[3] = proxSensors.countsRightWithRightLeds();
|
334
|
|
- compass.read();
|
335
|
|
- gyro.read();
|
|
381
|
+ TurnSensorUpdate();
|
|
382
|
+ MoveSensorUpdate();
|
336
|
383
|
|
337
|
384
|
//Funktionsauswahl
|
338
|
|
- if((compass.a.x - compassOffset) > 16000) CollisionDetection();
|
|
385
|
+/* if((compass.a.x - compassOffset) > 4000) CollisionDetection();
|
339
|
386
|
else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
|
340
|
387
|
else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200))
|
341
|
388
|
&& (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
|
342
|
|
- else TrackKeeper();
|
|
389
|
+ else TrackKeeper(); */
|
343
|
390
|
|
344
|
391
|
//Ausgabe über Bluetoothverbindung
|
345
|
392
|
SerialOutput();
|