|
@@ -8,7 +8,7 @@
|
8
|
8
|
//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden.
|
9
|
9
|
//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die
|
10
|
10
|
//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth).
|
11
|
|
-//das LCD kann bei Bluetoothkommunikation nicht genutzt werden.
|
|
11
|
+//Das LCD kann bei Bluetoothnutzung nicht verwendet werden.
|
12
|
12
|
|
13
|
13
|
#include <Wire.h>
|
14
|
14
|
#include <Zumo32U4.h>
|
|
@@ -21,13 +21,16 @@ Zumo32U4Encoders encoders;
|
21
|
21
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
22
|
22
|
L3G gyro; //Drehbewegungssensor z-Achse
|
23
|
23
|
|
24
|
|
-uint16_t lineValues[3]; //von links (0) nach rechts (2)
|
|
24
|
+uint16_t lineValue[3]; //von links (0) nach rechts (2)
|
25
|
25
|
uint16_t lineOffset[3];
|
|
26
|
+uint16_t lineRaw[3];
|
26
|
27
|
|
27
|
|
-uint8_t proxValues[4]; //von links (0) nach rechts (3)
|
|
28
|
+uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
28
|
29
|
|
29
|
|
-int16_t countsLeft; //Encoder
|
30
|
|
-int16_t countsRight;
|
|
30
|
+int16_t countsLeft; //Encoder rechts
|
|
31
|
+int16_t wayLeft;
|
|
32
|
+int16_t countsRight; //Encoder links
|
|
33
|
+int16_t wayRight;
|
31
|
34
|
|
32
|
35
|
int32_t rotationAngle = 0; //Drehwinkel
|
33
|
36
|
int32_t turnAngle = 0;
|
|
@@ -50,16 +53,17 @@ char report[200]; //Ausgabe
|
50
|
53
|
/*-----------------------------------------------------------------*/
|
51
|
54
|
|
52
|
55
|
//Setup Liniensensoren
|
53
|
|
-void LineSensorSetup()
|
|
56
|
+void LineSetup()
|
54
|
57
|
{
|
55
|
58
|
ledYellow(1);
|
|
59
|
+ lineSensors.initThreeSensors();
|
56
|
60
|
|
57
|
61
|
//Kalibrierung
|
58
|
62
|
uint32_t total[3] = {0, 0, 0};
|
59
|
63
|
for(uint8_t i = 0; i < 120; i++)
|
60
|
64
|
{
|
61
|
|
- if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
|
62
|
|
- else motors.setSpeeds(-200, 200);
|
|
65
|
+ if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
|
|
66
|
+ else motors.setSpeeds(-maxSpeed, maxSpeed);
|
63
|
67
|
lineSensors.read(lineOffset);
|
64
|
68
|
total[0] += lineOffset[0];
|
65
|
69
|
total[1] += lineOffset[1];
|
|
@@ -73,12 +77,19 @@ void LineSensorSetup()
|
73
|
77
|
lineOffset[2] = total[2] / 120;
|
74
|
78
|
}
|
75
|
79
|
|
|
80
|
+//Setup Abstandssensoren
|
|
81
|
+void ProxSetup()
|
|
82
|
+{
|
|
83
|
+ proxSensors.initThreeSensors();
|
|
84
|
+}
|
|
85
|
+
|
76
|
86
|
//Setup Drehbewegungssensor
|
77
|
|
-void TurnSensorSetup()
|
|
87
|
+void GyroSetup()
|
78
|
88
|
{
|
79
|
89
|
ledYellow(1);
|
80
|
|
- //800Hz Ausgaberate
|
81
|
|
- gyro.writeReg(L3G::CTRL1, 0b11111111); //Tiefpassfilter bei 100Hz
|
|
90
|
+ gyro.init();
|
|
91
|
+
|
|
92
|
+ gyro.writeReg(L3G::CTRL1, 0b11111111); //Ausgaberate 800Hz, Tiefpassfilter 100Hz
|
82
|
93
|
gyro.writeReg(L3G::CTRL4, 0b00100000); //2000dps Auflösung
|
83
|
94
|
gyro.writeReg(L3G::CTRL5, 0b00000000); //Hochpassfilter ausgeschaltet
|
84
|
95
|
|
|
@@ -90,14 +101,18 @@ void TurnSensorSetup()
|
90
|
101
|
gyro.read();
|
91
|
102
|
total += gyro.g.z;
|
92
|
103
|
}
|
93
|
|
- ledYellow(0);
|
94
|
104
|
gyroOffset = total / 1024;
|
|
105
|
+
|
|
106
|
+ gyroLastUpdate = micros();
|
|
107
|
+ ledYellow(0);
|
95
|
108
|
}
|
96
|
109
|
|
97
|
110
|
//Setup Beschleunigungssensor
|
98
|
|
-void MoveSensorSetup()
|
|
111
|
+void CompassSetup()
|
99
|
112
|
{
|
100
|
113
|
ledYellow(1);
|
|
114
|
+ compass.init();
|
|
115
|
+ compass.enableDefault();
|
101
|
116
|
|
102
|
117
|
//Kalibrierung
|
103
|
118
|
int32_t total = 0;
|
|
@@ -106,32 +121,28 @@ void MoveSensorSetup()
|
106
|
121
|
compass.read();
|
107
|
122
|
total += compass.a.x;
|
108
|
123
|
}
|
109
|
|
- ledYellow(0);
|
110
|
124
|
compassOffset = total / 1024;
|
|
125
|
+
|
|
126
|
+ compassLastUpdate = micros();
|
|
127
|
+ ledYellow(0);
|
111
|
128
|
}
|
112
|
129
|
|
|
130
|
+/*-----------------------------------------------------------------*/
|
|
131
|
+
|
113
|
132
|
void setup()
|
114
|
133
|
{
|
115
|
134
|
//Initialisierung der Bluetoothverbindung
|
116
|
135
|
Serial1.begin(9600);
|
117
|
136
|
if(Serial1.available()) Serial1.println("bluetooth available");
|
118
|
137
|
|
119
|
|
- //Initialisierung der Sensoren
|
120
|
|
- lineSensors.initThreeSensors();
|
121
|
|
- proxSensors.initThreeSensors();
|
122
|
|
- Wire.begin();
|
123
|
|
- compass.init();
|
124
|
|
- compass.enableDefault();
|
125
|
|
- gyro.init();
|
126
|
|
-
|
127
|
|
- //Kalibrierung der Sensoren
|
128
|
|
- Serial1.println("sensor calibration, dont touch");
|
|
138
|
+ //Initialisierung und Kalibrierung der Sensoren
|
|
139
|
+ Serial1.println("sensor calibration, dont touch");
|
|
140
|
+ Wire.begin();
|
129
|
141
|
delay(500);
|
130
|
|
- LineSensorSetup();
|
131
|
|
- TurnSensorSetup();
|
132
|
|
- gyroLastUpdate = micros();
|
133
|
|
- MoveSensorSetup();
|
134
|
|
- compassLastUpdate = micros();
|
|
142
|
+ ProxSetup();
|
|
143
|
+ LineSetup();
|
|
144
|
+ GyroSetup();
|
|
145
|
+ CompassSetup();
|
135
|
146
|
|
136
|
147
|
//Zumo bereit zu starten
|
137
|
148
|
Serial1.println("Zumo ready to start, press A");
|
|
@@ -141,8 +152,27 @@ void setup()
|
141
|
152
|
|
142
|
153
|
/*-----------------------------------------------------------------*/
|
143
|
154
|
|
|
155
|
+//Update Abstandssensoren
|
|
156
|
+void ProxUpdate()
|
|
157
|
+{
|
|
158
|
+ proxSensors.read();
|
|
159
|
+ proxValue[0] = proxSensors.countsLeftWithLeftLeds();
|
|
160
|
+ proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
|
161
|
+ proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
|
162
|
+ proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
163
|
+}
|
|
164
|
+
|
|
165
|
+//Updaten Liniensensoren
|
|
166
|
+void LineUpdate()
|
|
167
|
+{
|
|
168
|
+ lineSensors.read(lineRaw);
|
|
169
|
+ lineValue[0] = lineRaw[0] - lineOffset[0];
|
|
170
|
+ lineValue[1] = lineRaw[1] - lineOffset[1];
|
|
171
|
+ lineValue[2] = lineRaw[2] - lineOffset[2];
|
|
172
|
+}
|
|
173
|
+
|
144
|
174
|
//Update Drehbewegungssensor
|
145
|
|
-void TurnSensorUpdate()
|
|
175
|
+void GyroUpdate()
|
146
|
176
|
{
|
147
|
177
|
gyro.read();
|
148
|
178
|
turnRate = gyro.g.z - gyroOffset;
|
|
@@ -155,7 +185,7 @@ void TurnSensorUpdate()
|
155
|
185
|
}
|
156
|
186
|
|
157
|
187
|
// Update Beschleunigungssensor
|
158
|
|
-void MoveSensorUpdate()
|
|
188
|
+void CompassUpdate()
|
159
|
189
|
{
|
160
|
190
|
compass.read();
|
161
|
191
|
moveRate = compass.a.x - compassOffset;
|
|
@@ -168,16 +198,13 @@ void MoveSensorUpdate()
|
168
|
198
|
}
|
169
|
199
|
|
170
|
200
|
//Update Encoder
|
171
|
|
-//12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
201
|
+
|
172
|
202
|
void EncoderUpdate()
|
173
|
203
|
{
|
174
|
|
- static uint8_t lastDisplayTime;
|
175
|
|
- if ((uint8_t)(millis() - lastDisplayTime) >= 100)
|
176
|
|
- {
|
177
|
|
- lastDisplayTime = millis();
|
178
|
|
- countsLeft = encoders.getCountsLeft();
|
179
|
|
- countsRight = encoders.getCountsRight();
|
180
|
|
- }
|
|
204
|
+ countsLeft = encoders.getCountsLeft();
|
|
205
|
+ wayLeft = countsRight / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
206
|
+ countsRight = encoders.getCountsRight();
|
|
207
|
+ wayRight = countsRight / 910;
|
181
|
208
|
}
|
182
|
209
|
|
183
|
210
|
/*-----------------------------------------------------------------*/
|
|
@@ -204,36 +231,36 @@ void ObstacleAvoidance()
|
204
|
231
|
|
205
|
232
|
//Schritt 1: links bis über Mittellinie fahren
|
206
|
233
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
207
|
|
- turnAngle = 0;
|
208
|
|
- TurnSensorUpdate();
|
209
|
|
- while(lineValues[2] < (lineOffset[2] -200))
|
|
234
|
+ rotationAngle = 0;
|
|
235
|
+ GyroUpdate();
|
|
236
|
+ while(lineValue[2] > 1000)
|
210
|
237
|
{
|
211
|
|
- lineSensors.read(lineValues);
|
|
238
|
+ LineUpdate();
|
212
|
239
|
}
|
213
|
240
|
|
214
|
241
|
//Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
215
|
242
|
motors.setSpeeds(maxSpeed, maxSpeed/2);
|
216
|
|
- while(turnAngle != 0)
|
|
243
|
+ while(rotationAngle != 0)
|
217
|
244
|
{
|
218
|
|
- TurnSensorUpdate();
|
|
245
|
+ GyroUpdate();
|
219
|
246
|
}
|
220
|
247
|
|
221
|
248
|
//Gegenverkehr beachten
|
222
|
249
|
proxSensors.read();
|
223
|
|
- proxValues[1] = proxSensors.countsFrontWithLeftLeds();
|
224
|
|
- proxValues[2] = proxSensors.countsFrontWithRightLeds();
|
225
|
|
- if((proxValues[1] || proxValues[2]) < 4)
|
|
250
|
+ proxValue[1] = proxSensors.countsFrontWithLeftLeds();
|
|
251
|
+ proxValue[2] = proxSensors.countsFrontWithRightLeds();
|
|
252
|
+ if((proxValue[1] || proxValue[2]) < 4)
|
226
|
253
|
{
|
227
|
254
|
//Schritt 3: geradeaus fahren bis Hindernis passiert
|
228
|
255
|
maxSpeed = 400;
|
229
|
256
|
motors.setSpeeds(maxSpeed, maxSpeed);
|
230
|
257
|
delay(1000);
|
231
|
258
|
proxSensors.read();
|
232
|
|
- proxValues[3] = proxSensors.countsRightWithRightLeds();
|
233
|
|
- while(proxValues[3] > 4)
|
|
259
|
+ proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
260
|
+ while(proxValue[3] > 4)
|
234
|
261
|
{
|
235
|
262
|
proxSensors.read();
|
236
|
|
- proxValues[3] = proxSensors.countsRightWithRightLeds();
|
|
263
|
+ proxValue[3] = proxSensors.countsRightWithRightLeds();
|
237
|
264
|
TrackKeeper();
|
238
|
265
|
}
|
239
|
266
|
maxSpeed = 200;
|
|
@@ -242,18 +269,18 @@ void ObstacleAvoidance()
|
242
|
269
|
//Schritt 4: rechts bis über Mittellinie fahren
|
243
|
270
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
244
|
271
|
ledYellow(1);
|
245
|
|
- turnAngle = 0;
|
246
|
|
- TurnSensorUpdate();
|
247
|
|
- while(lineValues[0] < (lineOffset[0] -200))
|
|
272
|
+ rotationAngle = 0;
|
|
273
|
+ GyroUpdate();
|
|
274
|
+ while(lineValue[0] - 200)
|
248
|
275
|
{
|
249
|
|
- lineSensors.read(lineValues);
|
|
276
|
+ LineUpdate();
|
250
|
277
|
}
|
251
|
278
|
|
252
|
279
|
//Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
253
|
280
|
motors.setSpeeds(maxSpeed/2, maxSpeed);
|
254
|
|
- while(turnAngle != 0)
|
|
281
|
+ while(rotationAngle != 0)
|
255
|
282
|
{
|
256
|
|
- TurnSensorUpdate();
|
|
283
|
+ GyroUpdate();
|
257
|
284
|
}
|
258
|
285
|
}
|
259
|
286
|
|
|
@@ -262,7 +289,7 @@ void TrackKeeper()
|
262
|
289
|
{
|
263
|
290
|
|
264
|
291
|
//linke Linie erreicht, nach rechts fahren
|
265
|
|
- if(lineValues[0] > (lineOffset[0] + 200))
|
|
292
|
+ if(lineValue[0] < 1000)
|
266
|
293
|
{
|
267
|
294
|
motors.setSpeeds(maxSpeed, 0);
|
268
|
295
|
ledYellow(1);
|
|
@@ -271,7 +298,7 @@ void TrackKeeper()
|
271
|
298
|
}
|
272
|
299
|
|
273
|
300
|
//rechte Linie erreicht, nach links fahren
|
274
|
|
- else if(lineValues[2] > (lineOffset[2] + 200))
|
|
301
|
+ else if(lineValue[2] < 1000)
|
275
|
302
|
{
|
276
|
303
|
motors.setSpeeds(0, maxSpeed);
|
277
|
304
|
ledYellow(1);
|
|
@@ -280,8 +307,9 @@ void TrackKeeper()
|
280
|
307
|
}
|
281
|
308
|
|
282
|
309
|
//Linie überfahren, zurücksetzen
|
283
|
|
- else if(lineValues[1] > (lineOffset[1] + 200))
|
|
310
|
+ else if(lineValue[1] < 1000)
|
284
|
311
|
{
|
|
312
|
+ while(lineValue[0] > 1000)
|
285
|
313
|
motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
286
|
314
|
ledRed(1);
|
287
|
315
|
delay(1000);
|
|
@@ -314,19 +342,19 @@ void Crossroad()
|
314
|
342
|
if(randy == 1)
|
315
|
343
|
{
|
316
|
344
|
//zur Kreuzungsmitte fahren
|
317
|
|
- while((countsLeft != 200) && (countsRight != 200))
|
|
345
|
+ while((wayLeft != 1) && (wayRight != 1))
|
318
|
346
|
{
|
319
|
347
|
EncoderUpdate();
|
320
|
348
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
321
|
349
|
}
|
322
|
350
|
|
323
|
351
|
//links drehen
|
324
|
|
- turnAngle = 0;
|
325
|
|
- TurnSensorUpdate();
|
326
|
|
- while(turnAngle != 90)
|
|
352
|
+ rotationAngle = 0;
|
|
353
|
+ GyroUpdate();
|
|
354
|
+ while(rotationAngle != 90)
|
327
|
355
|
{
|
328
|
356
|
motors.setSpeeds(0, maxSpeed);
|
329
|
|
- TurnSensorUpdate();
|
|
357
|
+ GyroUpdate();
|
330
|
358
|
}
|
331
|
359
|
|
332
|
360
|
//geradeaus fahren
|
|
@@ -337,12 +365,12 @@ void Crossroad()
|
337
|
365
|
else if(randy == 2)
|
338
|
366
|
{
|
339
|
367
|
//rechts drehen
|
340
|
|
- turnAngle = 0;
|
341
|
|
- TurnSensorUpdate();
|
342
|
|
- while(turnAngle != 90)
|
|
368
|
+ rotationAngle = 0;
|
|
369
|
+ GyroUpdate();
|
|
370
|
+ while(rotationAngle != 90)
|
343
|
371
|
{
|
344
|
372
|
motors.setSpeeds(maxSpeed, 0);
|
345
|
|
- TurnSensorUpdate();
|
|
373
|
+ GyroUpdate();
|
346
|
374
|
}
|
347
|
375
|
|
348
|
376
|
//geradeaus fahren
|
|
@@ -351,22 +379,13 @@ void Crossroad()
|
351
|
379
|
}
|
352
|
380
|
|
353
|
381
|
//Funktion Serielle Ausgabe
|
354
|
|
-/* void SerialOutput()
|
355
|
|
-{
|
356
|
|
- snprintf_P(report, sizeof(report),
|
357
|
|
- PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Beschleunigung: %6d Winkel: %6d"),
|
358
|
|
- proxValues[0], proxValues[1], proxValues[2], proxValues[3],
|
359
|
|
- lineValues[0], lineValues[1], lineValues[2],
|
360
|
|
- dir, moveDisplay, rotationAngle);
|
361
|
|
- Serial1.println(report);
|
362
|
|
-} */
|
363
|
|
-
|
364
|
382
|
void SerialOutput()
|
365
|
383
|
{
|
366
|
384
|
snprintf_P(report, sizeof(report),
|
367
|
|
- PSTR("Winkel: %6d %6d %6d %6d %6u Weg: %6d %6d %6d %6d %6u"),
|
368
|
|
- rotationAngle, turnAngle, turnRate, gyroOffset, gyroLastUpdate,
|
369
|
|
- moveDisplay, moveDistance, moveRate, compassOffset, compassLastUpdate);
|
|
385
|
+ PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Weg: %6d %6d Winkel: %6d"),
|
|
386
|
+ proxValue[0], proxValue[1], proxValue[2], proxValue[3],
|
|
387
|
+ lineValue[0], lineValue[1], lineValue[2],
|
|
388
|
+ dir, wayLeft, wayRight, rotationAngle);
|
370
|
389
|
Serial1.println(report);
|
371
|
390
|
}
|
372
|
391
|
|
|
@@ -379,20 +398,16 @@ void loop()
|
379
|
398
|
ledRed(0);
|
380
|
399
|
|
381
|
400
|
//Abfragen der Sensordaten
|
382
|
|
- lineSensors.read(lineValues);
|
383
|
|
- proxSensors.read();
|
384
|
|
- proxValues[0] = proxSensors.countsLeftWithLeftLeds();
|
385
|
|
- proxValues[1] = proxSensors.countsFrontWithLeftLeds();
|
386
|
|
- proxValues[2] = proxSensors.countsFrontWithRightLeds();
|
387
|
|
- proxValues[3] = proxSensors.countsRightWithRightLeds();
|
388
|
|
- TurnSensorUpdate();
|
389
|
|
- MoveSensorUpdate();
|
|
401
|
+ LineUpdate();
|
|
402
|
+ ProxUpdate();
|
|
403
|
+ GyroUpdate();
|
|
404
|
+ CompassUpdate();
|
|
405
|
+ EncoderUpdate();
|
390
|
406
|
|
391
|
407
|
//Funktionsauswahl
|
392
|
|
-/* if((compass.a.x - compassOffset) > 4000) CollisionDetection();
|
393
|
|
- else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
|
394
|
|
- else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200))
|
395
|
|
- && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
|
|
408
|
+/* if((compass.a.x - compassOffset) > 1000) CollisionDetection();
|
|
409
|
+ else if((proxValue[0] || proxValue[1]) > 4) ObstacleAvoidance();
|
|
410
|
+ else if((lineValue[0] < 1000)) && (lineValue[1] < 1000)) && (lineValue[2] < 1000))) Crossroad();
|
396
|
411
|
else TrackKeeper(); */
|
397
|
412
|
|
398
|
413
|
//Ausgabe über Bluetoothverbindung
|