|
@@ -2,7 +2,7 @@
|
2
|
2
|
#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
3
|
3
|
#line 1 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
4
|
4
|
//Verfassser: Felix Stange, 4056379, MET2016
|
5
|
|
-//Datum: erstellt am 19.07.2017, zuletzt geändert am 08.05.2018
|
|
5
|
+//Datum: erstellt am 19.07.2017, zuletzt geändert am 14.05.2018
|
6
|
6
|
//Projektthema: Untersuchungen und Implementierung eines automatisierten Fahrens mittels Pololu Zumo
|
7
|
7
|
/*Kurzbeschreibung: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe
|
8
|
8
|
der Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
@@ -29,10 +29,10 @@ L3G gyro; //Drehbewegungssensor z-Achse
|
29
|
29
|
MedianFilter LineFilter0(3, 0); //erstellen der Filter für Liniensensoren
|
30
|
30
|
MedianFilter LineFilter1(3, 0); //Medianfilter geben mittleren Wert einer Reihe aus
|
31
|
31
|
MedianFilter LineFilter2(3, 0); //Fenstergröße: 3, Basiswerte: 0 0 0
|
32
|
|
-MedianFilter ProxFilter0(3, 0); //erstellen der Filter für Abstandssensoren
|
33
|
|
-MedianFilter ProxFilter1(3, 0);
|
34
|
|
-MedianFilter ProxFilter2(3, 0);
|
35
|
|
-MedianFilter ProxFilter3(3, 0);
|
|
32
|
+MedianFilter ProxFilter0(5, 0); //erstellen der Filter für Abstandssensoren
|
|
33
|
+MedianFilter ProxFilter1(5, 0);
|
|
34
|
+MedianFilter ProxFilter2(5, 0);
|
|
35
|
+MedianFilter ProxFilter3(5, 0);
|
36
|
36
|
|
37
|
37
|
#define MARKLINE0 150
|
38
|
38
|
#define MARKLINE1 100
|
|
@@ -80,43 +80,43 @@ bool alarm = false; //zeigt Manöver abhängig von btData
|
80
|
80
|
//Setup Bluetoothverbindung
|
81
|
81
|
#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
82
|
82
|
void BlueSetup();
|
83
|
|
-#line 87 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
83
|
+#line 88 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
84
|
84
|
void LineSetup();
|
85
|
|
-#line 116 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
85
|
+#line 117 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
86
|
86
|
void ProxSetup();
|
87
|
|
-#line 122 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
87
|
+#line 123 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
88
|
88
|
void GyroSetup();
|
89
|
|
-#line 155 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
89
|
+#line 156 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
90
|
90
|
void CompassSetup();
|
91
|
|
-#line 186 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
91
|
+#line 187 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
92
|
92
|
void setup();
|
93
|
|
-#line 212 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
93
|
+#line 213 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
94
|
94
|
void TimeUpdate();
|
95
|
|
-#line 218 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
95
|
+#line 219 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
96
|
96
|
void LoopTiming();
|
97
|
|
-#line 262 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
97
|
+#line 263 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
98
|
98
|
void ProxUpdate();
|
99
|
|
-#line 272 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
99
|
+#line 294 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
100
|
100
|
void LineUpdate();
|
101
|
|
-#line 282 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
101
|
+#line 304 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
102
|
102
|
void GyroUpdate();
|
103
|
|
-#line 295 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
103
|
+#line 317 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
104
|
104
|
void CompassUpdate();
|
105
|
|
-#line 308 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
105
|
+#line 330 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
106
|
106
|
void EncoderUpdate();
|
107
|
|
-#line 319 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
107
|
+#line 341 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
108
|
108
|
void Kontaktvermeiden();
|
109
|
|
-#line 338 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
109
|
+#line 360 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
110
|
110
|
void Hindernisumfahren();
|
111
|
|
-#line 464 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
111
|
+#line 486 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
112
|
112
|
void Abbiegen();
|
113
|
|
-#line 625 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
113
|
+#line 647 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
114
|
114
|
void Spurhalten();
|
115
|
|
-#line 670 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
115
|
+#line 692 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
116
|
116
|
void Spurfinden();
|
117
|
|
-#line 688 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
117
|
+#line 710 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
118
|
118
|
void SerielleAusgabe();
|
119
|
|
-#line 703 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
|
119
|
+#line 725 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
120
|
120
|
void loop();
|
121
|
121
|
#line 78 "c:\\Users\\Carolin\\Downloads\\VSCode\\Zumo32U4\\Zumo32U4\\Hauptprojekt\\Hauptprojekt.ino"
|
122
|
122
|
void BlueSetup()
|
|
@@ -125,6 +125,7 @@ void BlueSetup()
|
125
|
125
|
Serial1.setTimeout(10); //verkürzt Serial(1).read auf 10 ms statt 1000 ms
|
126
|
126
|
if(Serial1.available()) Serial.println("Bluetoothverbindung hergestellt");
|
127
|
127
|
if(Serial1.available() > 0) Serial1.read(); //Verwerfen der alten Informationen aus dem Puffer
|
|
128
|
+ Serial1.print(0);
|
128
|
129
|
}
|
129
|
130
|
|
130
|
131
|
//Setup Liniensensoren
|
|
@@ -305,11 +306,32 @@ void LoopTiming()
|
305
|
306
|
//Update Abstandssensoren
|
306
|
307
|
void ProxUpdate()
|
307
|
308
|
{
|
308
|
|
- proxSensors.read();
|
309
|
|
- proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
|
310
|
|
- proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
|
311
|
|
- proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
|
312
|
|
- proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
|
|
309
|
+ bool done = false;
|
|
310
|
+ int state = LOW;
|
|
311
|
+ while(done == false)
|
|
312
|
+ {
|
|
313
|
+ //wiederholt abfragen, ob bereits gesendet wird (Dauer je 1000 us bzw. 1 ms)
|
|
314
|
+ if(pulseIn(20, state, 1000) == 0 && pulseIn(22, state, 1000) == 0 && pulseIn(4, state, 1000) == 0)
|
|
315
|
+ //if(pulseIn(22, state, 1000) > 0)
|
|
316
|
+ {
|
|
317
|
+ //wenn nichts gesendet wird, selbst Messung durchführen
|
|
318
|
+ proxSensors.read();
|
|
319
|
+ proxValue[0] = ProxFilter0.in(proxSensors.countsLeftWithLeftLeds());
|
|
320
|
+ proxValue[1] = ProxFilter1.in(proxSensors.countsFrontWithLeftLeds());
|
|
321
|
+ proxValue[2] = ProxFilter2.in(proxSensors.countsFrontWithRightLeds());
|
|
322
|
+ proxValue[3] = ProxFilter3.in(proxSensors.countsRightWithRightLeds());
|
|
323
|
+ // Serial.println(proxValue[0]);
|
|
324
|
+ // Serial.println(proxValue[1]);
|
|
325
|
+ // Serial.println(proxValue[2]);
|
|
326
|
+ // Serial.println(proxValue[3]);
|
|
327
|
+ done = true;
|
|
328
|
+ }
|
|
329
|
+ else
|
|
330
|
+ {
|
|
331
|
+ //solange fremde Signale empfangen werden, Fehler ausgeben
|
|
332
|
+ Serial.println("Fremdmessung erkannt");
|
|
333
|
+ }
|
|
334
|
+ }
|
313
|
335
|
}
|
314
|
336
|
|
315
|
337
|
//Update Liniensensoren
|