-
Notifications
You must be signed in to change notification settings - Fork 0
/
VOR Dokumentation.html
644 lines (511 loc) · 17.5 KB
/
VOR Dokumentation.html
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
<!doctype html>
<html>
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0, user-scalable=yes">
<style>
/* CSS for Markdown text - v0.1 - https://gist.github.com/jbeyerstedt/133a7e9c00c8eb68aedb*/
body {
font-family: Helvetica, arial, sans-serif;
font-size: 14px;
line-height: 1.4;
margin: 0;
background-color: white; }
@media only screen and (max-width: 699px) {
body {
padding: 5px; }
}
@media only screen and (min-width: 700px) {
body {
padding: 30px;
max-width: 900px;
margin: 0 auto; }
}
body > *:first-child {
margin-top: 0 !important; }
body > *:last-child {
margin-bottom: 0 !important; }
a {
color: #4183C4; }
a.absent {
color: #cc0000; }
a.anchor {
display: block;
padding-left: 30px;
margin-left: -30px;
cursor: pointer;
position: absolute;
top: 0;
left: 0;
bottom: 0; }
h1, h2, h3, h4, h5, h6 {
margin: 20px 0 10px;
padding: 0;
font-weight: bold;
-webkit-font-smoothing: antialiased;
cursor: text;
position: relative; }
h1:hover a.anchor, h2:hover a.anchor, h3:hover a.anchor, h4:hover a.anchor, h5:hover a.anchor, h6:hover a.anchor {
background: url("../../images/modules/styleguide/para.png") no-repeat 10px center;
text-decoration: none; }
h1 tt, h1 code,
h2 tt, h2 code,
h3 tt, h3 code,
h4 tt, h4 code,
h5 tt, h5 code,
h6 tt, h6 code {
font-size: inherit; }
h1 {
font-size: 28px;
color: black; }
h2 {
font-size: 24px;
border-bottom: 1px solid #cccccc;
color: black; }
h3 {
font-size: 18px; }
h4 {
font-size: 16px;
margin-bottom: 0; }
h5 {
font-size: 14px; }
h6 {
color: #777777;
font-size: 14px; }
h5, h6 {
margin: 0; }
p, blockquote, ul, ol, dl, li, table, pre {
margin: 0 0 5px 0; }
hr {
border: 0 none;
padding: 0; }
body > h2:first-child {
margin-top: 0;
padding-top: 0; }
body > h1:first-child {
margin-top: 0;
padding-top: 0; }
body > h1:first-child + h2 {
margin-top: 0;
padding-top: 0; }
body > h3:first-child, body > h4:first-child, body > h5:first-child, body > h6:first-child {
margin-top: 0;
padding-top: 0; }
a:first-child h1, a:first-child h2, a:first-child h3, a:first-child h4, a:first-child h5, a:first-child h6 {
margin-top: 0;
padding-top: 0; }
h1 p, h2 p, h3 p, h4 p, h5 p, h6 p {
margin-top: 0; }
li p.first {
display: inline-block; }
li {
margin: 0; }
ul, ol {
padding-left: 10px;
margin-top: 4px;
margin-left: 15px; }
ul :first-child, ol :first-child {
margin-top: 0; }
ul :last-child, ol :last-child {
margin-bottom: 0; }
dl {
padding: 0; }
dl dt {
font-size: 14px;
font-weight: bold;
font-style: italic;
padding: 0;
margin: 15px 0 5px; }
dl dt:first-child {
padding: 0; }
dl dt > :first-child {
margin-top: 0; }
dl dt > :last-child {
margin-bottom: 0; }
dl dd {
margin: 0 0 15px;
padding: 0 15px; }
dl dd > :first-child {
margin-top: 0; }
dl dd > :last-child {
margin-bottom: 0; }
blockquote {
border-left: 4px solid #dddddd;
padding: 0 15px;
color: #777777; }
blockquote > :first-child {
margin-top: 0; }
blockquote > :last-child {
margin-bottom: 0; }
table {
padding: 0; }
table tr {
border-top: 1px solid #cccccc;
background-color: white;
margin: 0;
padding: 0; }
table tr:nth-child(2n) {
background-color: #f8f8f8; }
table tr th {
font-weight: bold;
border: 1px solid #cccccc;
text-align: left;
margin: 0;
padding: 6px 13px; }
table tr td {
border: 1px solid #cccccc;
text-align: left;
margin: 0;
padding: 6px 13px; }
table tr th :first-child, table tr td :first-child {
margin-top: 0; }
table tr th :last-child, table tr td :last-child {
margin-bottom: 0; }
img {
max-width: 100%; }
span.frame {
display: block;
overflow: hidden; }
span.frame > span {
border: 1px solid #dddddd;
display: block;
float: left;
overflow: hidden;
margin: 13px 0 0;
padding: 7px;
width: auto; }
span.frame span img {
display: block;
float: left; }
span.frame span span {
clear: both;
color: #333333;
display: block;
padding: 5px 0 0; }
span.align-center {
display: block;
overflow: hidden;
clear: both; }
span.align-center > span {
display: block;
overflow: hidden;
margin: 13px auto 0;
text-align: center; }
span.align-center span img {
margin: 0 auto;
text-align: center; }
span.align-right {
display: block;
overflow: hidden;
clear: both; }
span.align-right > span {
display: block;
overflow: hidden;
margin: 13px 0 0;
text-align: right; }
span.align-right span img {
margin: 0;
text-align: right; }
span.float-left {
display: block;
margin-right: 13px;
overflow: hidden;
float: left; }
span.float-left span {
margin: 13px 0 0; }
span.float-right {
display: block;
margin-left: 13px;
overflow: hidden;
float: right; }
span.float-right > span {
display: block;
overflow: hidden;
margin: 13px auto 0;
text-align: right; }
code, tt {
margin: 0 2px;
padding: 0 5px;
white-space: nowrap;
border: 1px solid #eaeaea;
background-color: #f8f8f8;
border-radius: 3px; }
pre code {
margin: 0;
padding: 0;
white-space: pre;
border: none;
background: transparent; }
.highlight pre {
background-color: #f8f8f8;
border: 1px solid #cccccc;
font-size: 13px;
line-height: 19px;
overflow: auto;
padding: 6px 10px;
border-radius: 3px; }
pre {
background-color: #f8f8f8;
border: 1px solid #cccccc;
font-size: 13px;
line-height: 19px;
overflow: auto;
padding: 6px 10px;
border-radius: 3px; }
pre code, pre tt {
background-color: transparent;
border: none; }
@media print {
body {
padding: 0;
margin: 1cm 1.5cm 2cm 2cm; }
hr {
page-break-before: always;
height: 1cm; }
h1, h2, h3, h4, h5, h6 {
page-break-after: avoid; }
table, pre {
page-break-inside: avoid; }
}
/* nested conters by https://gist.github.com/jirutka/32049196ab75547b7f47*/
ol {
list-style-type: none;
counter-reset: item;
margin: 0;
padding: 0; }
ol > li {
display: table;
counter-increment: item; }
ol > li:before {
content: counters(item, ".") ". ";
display: table-cell;
padding-right: 0.6em; }
li ol > li {
margin: 0; }
li ol > li:before {
content: counters(item, ".") " "; }
</style>
<title>VOR Sender und Empfänger - Dokumentation</title>
</head>
<body>
<h1>VOR Sender und Empfänger - Dokumentation</h1>
<h2>Glossar / Begriffsdefinitionen</h2>
<ul>
<li>VOR Quartal (quarter): 90° Segment des VOR</li>
<li>VOR-Master: VOR-Sender, der den Nordimpuls vorgibt</li>
<li>VOR-Slave(s): VOR-Sender, die dem Master folgen</li>
<li>Nordimpuls: Funksignal, das den Beginn einer VOR-Umdrehung anzeigt</li>
<li>Norden: bezeichnet nicht magnetisch oder geographisch Nord, sonden ist im Spielfeld definiert!</li>
<li>IR-Strahl (IR beam): Umlaufender Lichtstrahl</li>
<li>VOR Segmente (segments): Anzahl der umlaufenden Segmente (Anzahl IR-LEDs)</li>
</ul>
<h2>Die VOR-Technologie</h2>
<p>Die hier verwendete Technologie zur Positionsbestimmung ist dem sog. VOR aus der Luftfahrt angelehnt. Hierbei werden bekannt Punkte angepeilt, um seine eigene Position berechnen zu können.<br/>
Damit nun aber keine Kompasspeilungen vorgenommen werden müssen, gibt es einen Trick: Die Funkfeuer/ Sendetürme senden ein Umlaufendes Signal mit einem relativ kleinen Abstrahlwinkel. In dieser Implementation wird das umlaufende Signal mit IR-LEDs realisiert. Um nun aus diesem umlaufenden Strahl eine Winkelinformation zu erhalten, muss es noch eine Referenz geben. Diese wird durch einen allseitig abgegebenen Nordimpuls realisiert. Nun kann die Zeit zwischen dem Nordimpuls und der Messung den Strahl gemessen werden, die proportional zum Abstrahlwinkel ist. Die Peilung ist also immer zum Empfänger hin.<br/>
Da die Positionen der Sendetürme bekannt sind, kann aus mindestens zwei Peilungen eine Positions ermittelt werden.<br/>
Damit auch der Fall abgedeckt ist, dass ein Hindernis die Sicht auf einen Sendeturm verdeckt, gibt es drei Sendetürme.</p>
<hr />
<h2>Positionsberechnung</h2>
<h3>Spielfeld und Koordinatensystem</h3>
<pre><code> ^ Norden
|
[T1] ------------------ [T2]
| |
| |
| |
| (x) |
| |
| |
| |
(3m) | |
y ^ | |
| ------------------
┗--> x [T3]
(2m)
</code></pre>
<h3>Zuordnung Winkel - Sendeturm</h3>
<p>Je nach dem, aus welchem Winkel (bezogen auf Norden) der Strahl eintrifft, kann dieser eindeutig zu einem Sendeturm zugeordnet werden.</p>
<p>a) 0°- 90°: Sender 3<br/>
b) 90°-180°: Sender 1<br/>
c) 180°-270°: Sender 2<br/>
d) 270°-360°: Sender 3</p>
<p>Also muss in der Software die aus der Zeit berechnete Winkelangabe dem richtigen Sendetum zugeordnet werden.</p>
<h3>Berechnung</h3>
<p>Prinzipiell ergibt sich aus der Geometrie erst einmal ein LGS mit drei Gleichungen.</p>
<pre><code>Sender 1: g_1(x) = m_1*(x) + 3 mit: m_1 = tan(90°-α_1)
Sender 2: g_2(x) = m_2*(x-2) + 3 mit: m_2 = tan(90°-α_2)
==> g_2(x) = m_2*x - (2*m_2 - 3)
Sender 3: g_3(x) = m_3*(x-1) + 0 mit: m_3 = tan(90°-a_3)
==> g_3(x) = m_3*x - (1*m_3)
</code></pre>
<p>Andererseits kann nicht angenommen werden, dass sich alle drei Strahlen an einem Punkt schneiden, da schon die Anzahl der IR-LEDs nicht für eine hohe Genauigkeit ausreicht.</p>
<p>Daher müssen alle möglichen Schnittpunkte einzeln berechnet werden. Bei drei Sendern also drei Schnittpunkte, wenn ein Sender verdeckt ist, nur zwei Schnittpunkte.</p>
<hr />
<h3>Algorithmus</h3>
<p>Zur Berechnung der Position müssen folgende Schritte durchgeführt werden:</p>
<ul>
<li>Zeit zwischen Nordimpuls und Empfang des IR-Strahls messen</li>
<li>aus der Zeit den entsprechenden Winkel ermitteln</li>
<li>aus dem Winkel (oder der Zeit) auf den Sendeturm zurückschließen</li>
<li>aus dem Winkel und dem Sendeturm ergibt sich eine Geradengleichung</li>
<li>mit mind. zwei Geraden lässt sich eine Position errechnen</li>
</ul>
<h5>Ermittelung des Winkels:</h5>
<p>Bei einer Umlauffrequenz des IR-Strahls von <code>frequenz</code> Hz und einer Timer-Zeit (seit dem Nordimpuls) von <code>timer</code> Mikrosekunden ergibt sich ein Winkel <code>winkel</code> in Grad nach:</p>
<pre><code>winkel = 360 * timer / (1000000/frequenz)
</code></pre>
<h5>Berechnung der Geradenparameter:</h5>
<p>Die Geradenparameter ergeben sich aus den oben genannten drei Gleichungen. Allgemein gilt jedoch mit einer Turmposition von <code>(pos_x | pos_y)</code> und einem Winkel <code>alpha</code>:</p>
<pre><code>Bekannt: m (abhängig von alpha)
g(x) = m*(x - pos_x) + pos_y
==> g(x) = m * x - m * pos_x + pos_y = m * x + (pos_y - m*pos_x)
^ ^
Steigung Y-Achsenabschnitt
==> m = tan(90°-alpha)
b = pos_y - tan(90°-alpha)*pos_x
</code></pre>
<h5>Schnittpunkt zweier Geraden:</h5>
<p>Der Schnittpunkt zweier Geraden lässt sich berechnen, wenn die Steigung und der Y-Achsenabschnitt bekannt sind. Die Indizes <code>_1</code> und <code>_2</code> sollen hier die beiden Geraden unterscheiden.</p>
<pre><code>für Geradengleichungen der Form: y(x) = m*x + b
b_2 - b_1
x = ---------
m_1 - m_2
m_1 * b_2 - m_2 * b_1
y = -----------------------
m_1 - m_2
</code></pre>
<p>Implementierung als C Funktion:</p>
<pre><code>void CalcIntersection (float m_1, float m_2, float b_1, float b_2, float* p_x, float* p_y) {
*p_x = (b_2 - b_1) / (m_1 - m_2);
*p_y = ((m_1 * b_2) - (m_2 * b_1)) / (m_1 - m_2);
</code></pre>
<hr />
<h3>Mittelung der Werte</h3>
<p>Wenn ein Signal von allen drei Türmen empfangen wird, müssen auch drei Schnittpunkte der Geraden berechnet werden. Für die Berechnung des Mittelwertes bzw. -punktes gibt es potenziell zwei Methoden:</p>
<ul>
<li>Der Flächenschwerpunkt des Dreiecks</li>
<li>jeweils der Mittelwert der X- und Y-Kooridnaten der drei Punkte</li>
</ul>
<p>Wir haben uns für die einfache Methode - Mittelwert errechnen - entschieden. Der Flächenschwerpunkt spiegelt zwar für das ermittelte Dreieck besser den Mittelunkt wieder, hilft jedoch nicht undebingt dabei der realen Position näher zu kommen.</p>
<p>Nachdem nun nur noch ein Punkt zur Verfügung steht, wird dieser noch einmal über 5 Werte gemittelt bevor die Koordinaten an das Fahrzeug übergeben werden. So wird eine Datenrate von 2 Hz erreicht. Weitere Erklärungen dazu im nächsten Kapitel "Programmablauf".</p>
<hr />
<h2>Programmablauf</h2>
<p>Diese Parameter können eingestellt werden und werden im folgenden als Platzhalter verwendet:</p>
<ul>
<li>AVG_ANGLE 3</li>
<li>AVG_VALUES 2</li>
</ul>
<p>Um eine vorhersehbare Ausführungszeit zu haben, wird folgender Ablauf verwendet:</p>
<ul>
<li><code>(AVG_ANGLE-1)</code> Nordimpuls-Perioden Daten sammeln</li>
<li>danach eine Nordimpuls-Periode die Daten verarbeiten</li>
</ul>
<p>So werden Schwankungen in der Ausführungszeit, während die Daten gesammelt werden, vermieden - die Abtastzeit der IR-Signale bleibt also gleich.</p>
<h3>Hauptprogramm</h3>
<ul>
<li>Wenn Nordimpuls
<ul>
<li>dann Timer starten</li>
<li>Counter++</li>
</ul>
</li>
<li>Wenn Counter im Intervall [1, <code>(AVG_ANGLE-1)</code>]
<ul>
<li>Wenn irgendeine IR-LED empfangen
<ul>
<li>aktuellen Timerwert auslesen</li>
<li>Winkel aus Timer berechnen</li>
<li>aus dem Winkelbereich den korrekten Turm ermitteln</li>
<li>Winkel in die Turminstanz speichern</li>
</ul>
</li>
</ul>
</li>
<li>Wenn Counter = <code>AVG_ANGLE</code>
<ul>
<li>Wenn mind. 2 Türme Werte haben
<ul>
<li>Geradenparameter von der Turminstanz erhalten</li>
<li>Schnittpunkt(e) errechnen</li>
</ul>
</li>
<li>Werte über <code>AVG_VALUES</code> Mal sammeln
<ul>
<li>wenn <code>AVG_VALUES</code> gesammelt: Werte übertragen/ ausgeben</li>
<li>sonst: weiter sammeln</li>
</ul>
</li>
<li>Counter = 0</li>
</ul>
</li>
</ul>
<h3>C++-Klasse für die VOR-Sender (Türme)</h3>
<p>Die Turm-Klasse mittelt alle erhaltenen Winkel, wenn ein neuer Winkel eingegeben wird.<br/>
Erst beim Abrauf der Geradenparameter werden diese aus dem gemittelten Winkelwert errechnet. Dann wird auch der Winkel wieder zurückgesetzt</p>
<hr />
<h3>Werteausgabe</h3>
<p>Aufgrund der Nordimpuls-Frequenz von 50 Hz werden auch mit dieser Frequenz neue Daten gesammelt. Zur Glättung werden jeweils <code>(AVG_ANGLE-1)</code> Werte (in <code>AVG_ANGLE</code> Zyklen) zusammengefasst.<br/>
Wie oben schon kurz beschrieben, können die errechneten Positionen noch einmal über <code>AVG_VALUES</code> Werte gemittelt werden, bevor diese ausgegeben werden.</p>
<p>Zusätzlich kann mit <code>EN_FILTER_ARR</code> eine Filterung von unplausiblen Werten dazugeschaltet werden. Diese Option verwendet einen gleitenden Durchschnitt, daher sollte dann die Mittelung der Werte mit <code>AVG_VALUES</code> relativ klein eingestellt werden, da sonst die Berechnung für die Bewegung des Fahrzeugs zu träge ist.<br/>
Bei der Filterung wird mit <code>filterErrorCnt</code> noch geprüft, ob die Position zu häufig außerhalb der Toleranz lag. Dadurch wird verhindert, dass die Filterung an einer "falschen" Position festhängt. Der Ablauf ist dann wie folgt:</p>
<ul>
<li>wenn <code>filterErrorCnt</code> < <code>FILTER_ERR_THRES</code>
<ul>
<li>Mittelwert über das <code>filterArray</code> bilden</li>
<li>wenn Position weniger als <code>FILTER_DIFF_MAX</code> vom Mittelwert abweicht:
<ul>
<li>Werte im filterArray um eine Stelle nach hinten schieben</li>
<li>Position für die Ausgabe bleibt die aktuell errechnete Position</li>
<li><code>filterErrorCnt</code> = 0</li>
</ul>
</li>
<li>sonst:
<ul>
<li>Mittelwert für die Ausgabe verwenden</li>
<li><code>filterErrorCnt</code> hochzählen</li>
</ul>
</li>
</ul>
</li>
<li>wenn <code>filterErrorCnt</code> >= <code>FILTER_ERR_THRES</code>
<ul>
<li><code>filterArray</code> auf die aktuelle Position setzen --> d.h. reset</li>
</ul>
</li>
</ul>
<p>Die Serielle Datenausgabe (bzw. Datenübergabe) wurde von dem Team für das Fahrzeug entwickelt und getestet.</p>
<hr />
<h2>Hardware</h2>
<p>Dieses VOR-System besteht aus drei Sendern, und zwei Empfängern. Die Sender sind dabei miteinander gekoppelt und decken die 360° nur ein Mal ab. Es können beliebig weitere Empfänger hinzugefügt werden.</p>
<h3>Sender Hardware</h3>
<p>Der Sender besteht aus:</p>
<ul>
<li>Arduino nano</li>
<li>433 MHz Sender</li>
<li>(ein oder zwei) 8-Bit Schieberegister 74HC595 ??</li>
<li>(8 oder 16) IR-LEDs mit 40 kHz Modulation</li>
<li>(8 oder 16) Transistoren für die LEDs</li>
</ul>
<h3>Empfänger Hardware</h3>
<p>Der Empfänger besteht aus</p>
<ul>
<li>Arduino nano</li>
<li>433 MHz Empfänger</li>
<li>(8 oder 16) IR-Empfänger mit 40 kHz Modulation</li>
</ul>
<h3>433MHz Strecke</h3>
<p>Der Nordimpuls wird über eine 433 MHz Funkstecke übertragen. Diese hat eine Verzögerung zwischen Sendereinganz und Empfängerausgang von 40μs.</p>
<h2>Schnittstelle zum Roboter</h2>
<p>Dem Roboter werden die Positionsdaten über die Serielle Schnittstelle (UART) des Arduino übergeben.<br/>
Das Datenformat ist folgendes:</p>
<ul>
<li>Startzeichen <code>@</code></li>
<li>X-Koordinate (in cm) als 16 Bit unsigned int (zwei Bytes nacheiander)</li>
<li>Y-Koordinate (in cm) als 16 Bit unsigned int (zwei Bytes nacheiander)</li>
</ul>
<pre><code>Serial.print("@");
Serial.write((char)x>>8); Serial.write((char)x);
Serial.write((char)y>>8); Serial.write((char)y);
</code></pre>
</body>
</html>