|
@@ -1,7 +1,7 @@
|
1
|
1
|
//Verfassser: Felix Stange 4056379 MET2016
|
2
|
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
|
|
-//Liniensensoren (3), ähnlich einer Spurhalteautomatik (heller Belag und dunkle Streifen).
|
|
4
|
+//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und helle 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)
|
|
@@ -29,12 +29,14 @@ uint8_t proxValues[4]; //von links (0) nach rechts (3)
|
29
|
29
|
int16_t countsLeft; //Encoder
|
30
|
30
|
int16_t countsRight;
|
31
|
31
|
|
32
|
|
-uint16_t turnAngle = 0; //Drehwinkel
|
|
32
|
+int32_t rotationAngle = 0; //Drehwinkel
|
|
33
|
+int32_t turnAngle = 0;
|
33
|
34
|
int16_t turnRate;
|
34
|
35
|
int16_t gyroOffset;
|
35
|
36
|
uint16_t gyroLastUpdate;
|
36
|
37
|
|
37
|
|
-uint16_t moveDistance = 0; //Beschleunigung
|
|
38
|
+int32_t moveDisplay = 0; //Beschleunigung
|
|
39
|
+int32_t moveDistance = 0;
|
38
|
40
|
int16_t moveRate;
|
39
|
41
|
int16_t compassOffset;
|
40
|
42
|
uint16_t compassLastUpdate;
|
|
@@ -43,7 +45,7 @@ uint8_t randy; //Zufallszahl für Richtung
|
43
|
45
|
|
44
|
46
|
int maxSpeed = 200; //Maximale Geschwindigkeit (möglich 400)
|
45
|
47
|
char dir = '0'; //Fahrtrichtung, Ereignis
|
46
|
|
-char report[120]; //Ausgabe
|
|
48
|
+char report[200]; //Ausgabe
|
47
|
49
|
|
48
|
50
|
/*-----------------------------------------------------------------*/
|
49
|
51
|
|
|
@@ -51,7 +53,6 @@ char report[120]; //Ausgabe
|
51
|
53
|
void LineSensorSetup()
|
52
|
54
|
{
|
53
|
55
|
ledYellow(1);
|
54
|
|
- delay(500);
|
55
|
56
|
|
56
|
57
|
//Kalibrierung
|
57
|
58
|
uint32_t total[3] = {0, 0, 0};
|
|
@@ -76,7 +77,6 @@ void LineSensorSetup()
|
76
|
77
|
void TurnSensorSetup()
|
77
|
78
|
{
|
78
|
79
|
ledYellow(1);
|
79
|
|
- delay(500);
|
80
|
80
|
//800Hz Ausgaberate
|
81
|
81
|
gyro.writeReg(L3G::CTRL1, 0b11111111); //Tiefpassfilter bei 100Hz
|
82
|
82
|
gyro.writeReg(L3G::CTRL4, 0b00100000); //2000dps Auflösung
|
|
@@ -98,7 +98,6 @@ void TurnSensorSetup()
|
98
|
98
|
void MoveSensorSetup()
|
99
|
99
|
{
|
100
|
100
|
ledYellow(1);
|
101
|
|
- delay(500);
|
102
|
101
|
|
103
|
102
|
//Kalibrierung
|
104
|
103
|
int32_t total = 0;
|
|
@@ -126,8 +125,8 @@ void setup()
|
126
|
125
|
gyro.init();
|
127
|
126
|
|
128
|
127
|
//Kalibrierung der Sensoren
|
129
|
|
- Serial1.println("sensor calibration");
|
130
|
|
- buttonA.waitForButton();
|
|
128
|
+ Serial1.println("sensor calibration, dont touch");
|
|
129
|
+ delay(500);
|
131
|
130
|
LineSensorSetup();
|
132
|
131
|
TurnSensorSetup();
|
133
|
132
|
gyroLastUpdate = micros();
|
|
@@ -135,9 +134,8 @@ void setup()
|
135
|
134
|
compassLastUpdate = micros();
|
136
|
135
|
|
137
|
136
|
//Zumo bereit zu starten
|
138
|
|
- Serial1.println("Zumo ready to start");
|
|
137
|
+ Serial1.println("Zumo ready to start, press A");
|
139
|
138
|
buttonA.waitForButton();
|
140
|
|
-
|
141
|
139
|
delay(500);
|
142
|
140
|
}
|
143
|
141
|
|
|
@@ -153,7 +151,7 @@ void TurnSensorUpdate()
|
153
|
151
|
gyroLastUpdate = m;
|
154
|
152
|
int32_t d = (int32_t)turnRate * dt;
|
155
|
153
|
turnAngle += (int64_t)d * 14680064 / 17578125;
|
156
|
|
- turnAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
|
|
154
|
+ rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
|
157
|
155
|
}
|
158
|
156
|
|
159
|
157
|
// Update Beschleunigungssensor
|
|
@@ -165,8 +163,8 @@ void MoveSensorUpdate()
|
165
|
163
|
uint16_t dt = m - compassLastUpdate;
|
166
|
164
|
compassLastUpdate = m;
|
167
|
165
|
int32_t d = (int32_t)moveRate * dt;
|
168
|
|
- moveDistance += (int64_t)d * dt / 16384;
|
169
|
|
- moveDistance = (moveDistance * 1000) / 9,81;
|
|
166
|
+ moveDistance += (int64_t)d * dt >> 14;
|
|
167
|
+ moveDisplay = moveDistance * 1000 / 9,81;
|
170
|
168
|
}
|
171
|
169
|
|
172
|
170
|
//Update Encoder
|
|
@@ -353,13 +351,22 @@ void Crossroad()
|
353
|
351
|
}
|
354
|
352
|
|
355
|
353
|
//Funktion Serielle Ausgabe
|
356
|
|
-void SerialOutput()
|
|
354
|
+/* void SerialOutput()
|
357
|
355
|
{
|
358
|
356
|
snprintf_P(report, sizeof(report),
|
359
|
357
|
PSTR("Abstand: %3d %3d %3d %3d Linie: %6d %6d %6d Richtung: %3c Beschleunigung: %6d Winkel: %6d"),
|
360
|
358
|
proxValues[0], proxValues[1], proxValues[2], proxValues[3],
|
361
|
359
|
lineValues[0], lineValues[1], lineValues[2],
|
362
|
|
- dir, moveDistance, turnAngle);
|
|
360
|
+ dir, moveDisplay, rotationAngle);
|
|
361
|
+ Serial1.println(report);
|
|
362
|
+} */
|
|
363
|
+
|
|
364
|
+void SerialOutput()
|
|
365
|
+{
|
|
366
|
+ 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);
|
363
|
370
|
Serial1.println(report);
|
364
|
371
|
}
|
365
|
372
|
|