Liste der Anhänge anzeigen (Anzahl: 2)
Laser-Positionsscanner
Hallo Kreative,
wie bereits hier https://www.roboternetz.de/community...immung-im-Raum angekündigt will ich mal endlich anfangen………………..
Beschrieben wird ein Laserscanner für die absolute Positionsbestimmung Eurer Roboter. Dies ist (glaube ich) KEIN ANFÄNGERPROJEKT !!! :gift
Auch nicht gerade preiswert was die Hardware anbetrifft. Außerdem sind entsprechende Werkzeuge gefragt, die nicht jeder in der Bastelstube stehen hat.
Trotzdem nicht unmöglich dieses zu reproduzieren bzw. als Grundlage für eigene Ideen zu verwenden.
Alles ist noch nicht ausgearbeitet. Ich werde in lockeren Abständen erweitern! Das ganze Projekt läuft schon seit mehreren Jahren und ich muss teilweise richtig überlegen was ich da vor Jahren zusammengerührt habe. Bitte nicht drängeln………………
U.a. diese Quellen waren außer Oberallgeiers Beiträgen https://www.roboternetz.de/community...-Triangulation(nochmals vielen Dank an dieser Stelle) Lieferanten für dieses interessante und lehrreiche Projekt:
http://www.seattlerobotics.org/encoder/200109/lasernav.htm
http://www.philohome.com/sensors/lasersensor.htm
Wir beginnen mit etwas Theorie (Geometrie > Stichworte: Thales / Dreieck / Kreis):
Die Thales_x.pdf sind im Anhang versteckt. Es ist anzuraten diese vorher zu öffnen. Ein Bild sagt mehr.............. EDIT 03.10.2011 ! Es waren Fehler in den PDFs
Thales_1.pdf
Das grüne Rechteck ist die Spielfläche (z.B. damfino's Rasen). Die Punkte A, B und C stellen die Reflektoren dar. Die Abstände zwischen A und B respektive B und C müssen bekannt sein und werden dem µC als Variablen oder Konstanten mitgeteilt. Die Reflektoren müssen auf einer Geraden liegen (es geht auch in beliebigen Winkeln, diese Anordnung hat jedoch Vorteile, näheres später dazu).
Der gelbe Kreis stellt den Roboter dar, der kleine schwarze Pfeil seine Fahrtrichtung (Nase). Der Positionsscanner ist (praktischerweise) im Mittelpunkt angeordnet. Von hier aus werden die Winkel Alpha (zwischen A und B) , Beta (zwischen B und C) und Gamma (zwischen C und A) gemessen. Je genauer diese Winkel ermittelt werden umso höher die Auflösung der Absolut-Position (die Auflösung ist nicht gleichmäßig auf dem Feld verteilt!).
Als nächstes errichten wir lotrechte Linien bei A_B/2 und B_C/2. Diese Linien bilden später die X-Koordinaten für 2 Kreise mit deren Hilfe wir die Position des Laserscanners ermitteln.
Thales_2.pdf
Code:
Xm1 = T_ab / 2 : Xm2 = T_bc / 2 : Xm2 = Xm2 + T_ab
Die Kreise haben ihre X-Koordinaten immer auf diesen Hilfslinien und der Kreis läuft immer durch die Punkte A und B bzw. B und C.
Nun gilt es den Mittelpunkt (Y-Koordinate) der Kreise zu bestimmen. Dazu werden die gemessenen Winkel verdoppelt und damit die folgenden Dreiecke berechnet.
Thales_3.pdf
Code:
K1winkel2 = Beta * 2 : K2winkel2 = Alpha * 2 'Thaleskreis !
Thales_4.pdf
Code:
K1alpha = 180 -k1winkel2 : K1alpha = K1alpha / 2 'Mittelpunkt und Radius 1.Kreis
K1rad1 = Pi * K1alpha : K1rad1 = K1rad1 / 180
K1rad2 = Pi * K1winkel2 : K1rad2 = K1rad2 / 180
R1 = Sin(k1rad1) : R1 = R1 * T_ab : R1 = R1 / Sin(k1rad2)
Ym1 = Sin(k1rad1) : Ym1 = Ym1 * R1
K2alpha = 180 -k2winkel2 : K2alpha = K2alpha / 2 'Mittelpunkt und Radius 2.Kreis
K2rad1 = Pi * K2alpha : K2rad1 = K2rad1 / 180
K2rad2 = Pi * K2winkel2 : K2rad2 = K2rad2 / 180
R2 = Sin(k2rad1) : R2 = R2 * T_bc : R2 = R2 / Sin(k2rad2)
Ym2 = Sin(k2rad1) : Ym2 = Ym2 * R2
Da ich für hohe Mathematik zu dumm bin habe ich mich mal hier bedient: http://www.antonis.de/faq/progs (kreisnit.bas)
Nun brauchen wir nur noch die Kreise zu zeichnen :
Thales_5.pdf
Code:
If R1 < R2 Then 'der erste Radius muß größer als der 2. sein
Ht = Xm2 : Xm2 = Xm1 : Xm1 = Ht 'Swap
Ht = Ym2 : Ym2 = Ym1 : Ym1 = Ht
Ht = R2 : R2 = R1 : R1 = Ht
End If
Call Schnitt_kreis(xm1 , Ym1 , R1 , Xm2 , Ym2 , R2 , Xs1 , Ys1 , Xs2 , Ys2 , M) 'Schnittpunkte ermitteln
und die Schnittpunkte der beiden Kreise auszuwerten.
Ein Schnittpunkt liegt auf B (Y=0) und ist leicht auszusortieren :
Code:
I = Round(ys2)
If I = 0 Then 'Schnittpunkt liegt auf "B"
Ist_x_gemessen = Xs1 : Ist_y_gemessen = Ys1 'Schnittpunkt 1
Else
Ist_x_gemessen = Xs2 : Ist_y_gemessen = Ys2 'Schnittpunkt 2
End If
Und siehe da, der Roboter steht genau auf dem Schnittpunkt !!! War doch ganz easy, nicht war ? ;)
Als zusätzliche Information bekommt man den Winkel des Robbys im Raum. Dieser wird aus dem Winkel zum Reflektor B abgeleitet.
Nun haben wir alle Informationen um das Gefährt mit der Odometrie auf den (nächsten) Zielpunkt zu navigieren.
Thales_6.pdf
Weiter geht es mit dem eigentlichen Scanner (bald.....----->Theorie) ..............
Liste der Anhänge anzeigen (Anzahl: 2)
Da soweit alles klar ist geht es weiter mit der Hardware.........
Funktion Positionsscanner:
(Blockschaltbild.pdf im Anhang)
Der Scanner besteht mechanisch aus einem Rotor (grün) und einem feststehenden Teil (blau & grau).
Der Rotor ist (leicht) gebremst so dass das Getriebespiel nicht zum Tragen kommt.
Auf dem Rotor ist neben der Energieversorgung der modulierte Laser-Emitter und das Empfangsmodul untergebracht. Die empfangenen Signale werden über Infrarot durch die Antriebs(hohl)welle zum feststehenden Teil geleitet.
Am Rotor ist eine Zunge befestigt die den Nullpunkt (0°) im Zusammenhang mit dem Referenzsignal des Inkrementalgebers bildet. Mit diesem wird der Zähler zurückgesetzt.
Das ganze bildet einen durch die Bakenreflektoren getriggerten (hochauflösenden) Winkelmesser.
Durch den Inkrementalgeber wird der Zähler fortlaufend hochgezählt (im Beispiel von 0.....144000). Wird eine Bake erkannt wird über das Monoflop (MP2) dessen Zeitkonstante über der Periodendauer der Modulation liegt und über das Monoflop (MP4) das Latch getriggert und damit der momentane Winkel "eingefroren". Gleichzeitig kann der µC mit Int0 in die Interruptverarbeitung verzweigen und den Winkel über PortA abholen. Die Anzahl der 50KHz Impulse wird über Counter T1 gezählt. Es liegt nun an der Software das Ende des Impulspaketes festzustellen und mit PortB.3 (MP3) die Übernahme dieses Winkels erneut zu triggern und auszulesen.
Als endgültiger Winkel für die weiterführende Berechnung wird die Mitte zwischen Anfang und Ende der Bake herangezogen. Da die Bake ein rundes Profil hat und somit quasi der Mittelpunkt verwendet wird ist das ganze damit (weitgehend) Richtungsunabhängig.
Impulsdiagramm für ein Bakensignal (ideal) :
Anhang 20015
Fragen hierzu?
Als nächstes dann die konkrete Schaltung.............
\\:D/
Liste der Anhänge anzeigen (Anzahl: 5)
Die Lichtschranke:
Anhang 20066
(Lichtschranke_prinzip.pdf im Anhang)
Das rote Drehteil beinhaltet die Laserdiode (Kollimator) als Sender, einen Spiegel (45°) und den Empfänger nebst Sammellinse (90°) .
Zum Ausrichten der Komponenten zueinander ist das rote Teil auf der weißen Grundplatte drehbar angeordnet. Das ganze ist mittig auf der Platine aufgeklebt.
Auf der Unterseite der Platine sitzt die Infrarot-Sendediode und die Sekundärspule des Royerconverters.
Anhang 20067
Auf der Abbildung oben sind die beiden Spulenhälften des Royerconverters etwas auseinandergezogen. Unter der Grundplatte (grünes FR4) ist die Mechanik und die µC-Platine verborgen. In diesem Prototypen habe ich versucht den gesamten Scanner kardanisch aufzuhängen (Weißer Ring im runden roten Gehäuse). Damit soll sich der Scanner selbsttätig waagerecht ausrichten.
Anhang 20068
Links oben der Motor, vorn das Schneckengetriebe, rechts unten der Inkrementalgeber und in der Mitte die Hohlwelle durch die die Empfangssignale per IR geschickt werden.
Die Metallzunge auf dem Schneckenrad betätigt die Gabellichtschranke auf der µC-Platine (0°).
Anhang 20069
Die (Prototyp)-µC-Platine:
In der Mitte das Weiße Drehteil trägt eine Sammellinse für die IR-Empfangsdiode. Links daneben die Gabellichtschanke. Das Flachbandkabel verbindet zum Inkrementalgeber, ganz links unten der Motoranschluß und rechts daneben der Royerconverter. Links oben der Zähler und die Latches.
Liste der Anhänge anzeigen (Anzahl: 1)
....hat leider nicht mehr reingepasst:
Für die kardanische Aufhängung und das runde Gehäuse (KG-Rohr) hat sich diese Bauform als ungünstig herausgestellt. In den Eagle-Files (Anhang) ist das bereits geändert. Die Platine muß an der großen runden Markierung "gestutzt" werden.
Rechts oben der 8-polige Steckverbinder ist die Verbindung zum übergeordneten System.
Weiter dann mit der Software........................
Liste der Anhänge anzeigen (Anzahl: 1)
Nun zur Software.........
Funktion Winkelerfassung:
(Impulse_Software.pdf in .zip)
Wie schon weiter oben erwähnt kommen die Impulse leider nicht immer als geschlossene Formation an (ideal) sondern sind oft "zerhackt" (Reflektor verschmutzt o.ä.). Dies muss die Software ausbügeln:
Bei einer Umdrehung des Scanners werden die Winkelinformationen (in der Int0) in die Arrays "Wa(x)" >(für Winkelanfang) und "We(x)" >(für Winkelende) eingetütet (schwarz in der Abb.).
Code:
'--------------------------------------------------------------------------
Int0_int:
Counter1 = 0 'hier werden die empfangenen Impulse gezählt
Led = 0
Laser_anf_low = Pina : Portc.6 = 1 : Waitus 2 '&b00 lesen
Laser_anf_mid = Pina : Portc.6 = 0 : Portc.7 = 1 : Waitus 2 '&b01 lesen
Laser_anf_high = Pina : Portc.7 = 0 '&b10 lesen
Waitus 40 '~2 Impulse ausblenden
Bitwait Pind.2 , Reset 'Burst Ende
Lesezaehler = 1 'Zählerwert speichern
Waitus 5
Lesezaehler = 0
Waitus 5
Laser_end_low = Pina : Portc.6 = 1 : Waitus 2 '&b00 lesen
Laser_end_mid = Pina : Portc.6 = 0 : Portc.7 = 1 : Waitus 2 '&b01 lesen
Laser_end_high = Pina : Portc.7 = 0 '&b10 lesen
Waitus 2
Target_quali = Counter1 'Anzahl der empfangenen Impulse
Toggle Buzzer 'kurze akustische Rückmeldung
Incr Anz_target 'Anzahl der Impulspakete
Winkelanz(anz_target) = Target_quali 'Impulse > Array
Wa(anz_target) = Laser_anf 'Winkel_anfang > Array
We(anz_target) = Laser_ende 'Winkel_ende > Array
Waitus 40 'Wartezeit
Led = 1
Eifr.0 = 1 'evt. anstehenden Interrupt löschen
Return
Code:
'-------------------------------------------------------------------------------
Sub Positionserfassung 'hier werden die Empfangsdaten ausgewertet
If Umdrehung_beendet = 1 Then
Anz_target_fertig = 1 'Ältere BASCOM-Versionen können nicht mit Index "0" !
If Anz_target > 1 Then
Temp_wa(1) = Wa(1) 'Anfang merken
For N = 1 To Anz_target 'Schleife über alle Pakete
Diff = Wa(n + 1) - We(n) 'Differenz zwischen den Paketen
If Diff < 0 Then Diff = Diff + 144000 '"0"-Durchgang
If Diff > 0 And Diff < 700 Then '700 ist die Toleranz (~1,7°)
Winkelanz(anz_target_fertig) = Winkelanz(anz_target_fertig) + Winkelanz(n + 1) 'nur Impulse addieren
Else
Wa(anz_target_fertig) = Temp_wa(anz_target_fertig) 'Anfang eintüten
We(anz_target_fertig) = We(n) 'Ende eintüten
Templ1 = We(anz_target_fertig) - Wa(anz_target_fertig) 'Differenz
If Templ1 < 0 Then 'auf "0"-Durchgang prüfen
Templ1 = Templ1 + 144000 'ggf. korrigieren
End If
Templ1 = Templ1 \ 2
Templ1 = Templ1 + Wa(anz_target_fertig)
If Templ1 > 144000 Then '"0"-Durchgang ?
Templ1 = Templ1 - 144000 'ggf. korrigieren
End If
Winkel(anz_target_fertig) = Templ1 'endlich geschafft !
Incr Anz_target_fertig 'nächstes Paket
Temp_wa(anz_target_fertig) = Wa(n + 1) 'nächsten Anfang merken
Winkelanz(anz_target_fertig) = Winkelanz(n + 1) 'nächste Impulsanzahl merken
End If
Next N
End If
Decr Anz_target_fertig 's.o.
If Anz_target_fertig = 3 Then '!!!!!!! NUR 3 BAKEN ERGEBEN SINNVOLLE WERTE !!!!!!!!!
W1 = Winkel(1) \ 4 'für Übertragung zum MASTER bevor evtl. ge"swap"t wird !
W2 = Winkel(2) \ 4 'eigentlich wird nur Winkel(2) gebraucht
W3 = Winkel(3) \ 4
W1_word = W1 'für 16Bit-Übermittlung (TWI)
W2_word = W2
W3_word = W3
Call Winkel_sortieren
Call Positionsberechnung(t_ab , T_bc)
Lesefehler = 0 'Gut
Sound Buzzer , 3 , 3700 'Gut
Berechnen_winkel = Gamma \ 2 'wann kann ich loslegen ?
Berechnen_winkel = Berechnen_winkel * 400
Berechnen_winkel = Berechnen_winkel + Winkel(2)
If Berechnen_winkel > 144000 Then
Berechnen_winkel = Berechnen_winkel - 144000
End If
If Berechnen_winkel > 143360 Then Berechnen_winkel = 0 'letztes Stück passt nicht ganz
Berechnen_fenster = Berechnen_winkel + 2048
Else
Ist_x_gemessen = 0 : Ist_y_gemessen = 0
Incr Lesefehler
Sound Buzzer , 3 , 700 'Schlecht
End If
X_word = Ist_x_gemessen 'für 16Bit-Übermittlung (TWI)
Y_word = Ist_y_gemessen
Anz_target_merk = Anz_target
Quali_merk = 0
For N = 1 To 50
Quali_merk = Quali_merk + Winkelanz(n) 'für die Statistik
Next N
Portb.4 = 1 'Ich habe frische Daten für MASTER (HS)
Umdrehung_beendet = 0 '"aufräumen" hat die Mutti gesagt
Anz_target = 0
For N = 1 To 50
Winkelanz(n) = 0
Winkel(n) = 0
Wa(n) = 288000
Temp_wa(n) = 0
We(n) = 0
Next N
End If
End Sub
Nach dem Ereignis "Umdrehung_beendet" (blau in der Abb. >später mehr dazu) werden diese Arrays auf "Zusammenhang" abgefragt und das Resultat wieder ab Wa(1) & We(1) abgelegt.
Also steht jetzt im Array:
Wa(1) & We(1) >1.Paket
Wa(2) & We(2) >2.Paket
Wa(3) & We(3) >3.Paket
Nun müssen wir nur noch die Mitte ausrechnen:
[ Wa+((We-Wa)/2) ] kommt nach Winkel(x) (in der Abb. rot).
Weiterhin wird die Anzahl der zusammenhängenden Pakete in der Variablen Anz_Target_fertig abgelegt. Wenn diese den Inhalt 3 hat kann weitergearbeitet werden, wenn nicht > Pech gehabt !!!
Das Array "Winkelanz(x)" beinhaltet jetzt die Anzahl der Impulse pro Paket und gestattet es Rückschlüsse auf den Abstand zur Bake und/oder die Qualität der Reflexion zu ziehen.
Fragen hierzu ?
Als nächstes ist das Problem zu lösen:
"Der Roboter dreht sich ja auch !" (Sub Winkel_sortieren "Berechnen_winkel" / "Berechnen_fenster" u.s.w.)
Liste der Anhänge anzeigen (Anzahl: 1)
!!! Als erstes muss ich mich dafür entschuldigen das ich in den bisherigen PDFs die Winkel Alpha und Beta vertausch habe !!!
In diesem Beitrag ist das berücksichtigt.
Kann ich das nachträglich in den entspr. Beiträgen "geraderücken" ?>>>>>>>>> EDIT 03.10.2011 ! Schon erledigt !
Der Roboter dreht sich .........
Funktion Winkelauswertung:
(Winkelauswertung.pdf)
Hier geht es darum die Winkel Alpha, Beta und Gamma sicher und eindeutig zu bestimmen. Rohkost hierfür ist das Array Winkel(1..3). Durch die Anordnung der Baken in einer Linie ist gewährleistet das unabhängig vom Standpunkt des Scanners auf dem Spielfeld der Winkel Gamma immer größer als 180° ist. Dieser Umstand hilft uns den Algorithmus einfach und effektiv zu gestalten.
In den beiden Beispielen im PDF wurden die 3 Winkel Winkel(1), Winkel(2) und Winkel(3) ab 0° in aufsteigender Reihenfolge erfasst.
Winkel(1) korrespondiert allerdings nicht immer mit Bake C u.s.w. (z.B. wenn der Roboter sich gedeht hat) !
Code:
'-------------------------------------------------------------------------------
Sub Winkel_sortieren
Pruef = Winkel(2) - Winkel(1)
If Pruef > 72000 Then
Swap Winkel(1) , Winkel(3) '0° in Beta
Swap Winkel(1) , Winkel(2)
End If
Pruef = Winkel(3) - Winkel(2)
If Pruef > 72000 Then
Swap Winkel(1) , Winkel(3) '0° in Alpha
Swap Winkel(2) , Winkel(3)
End If
Alpha = Winkel(2) - Winkel(1)
Sig = Sgn(alpha)
If Sig = -1 Then
Alpha = 144000 + Alpha
End If
Beta = Winkel(3) - Winkel(2)
Sig = Sgn(beta)
If Sig = -1 Then
Beta = 144000 + Beta
End If
Gamma = Winkel(1) - Winkel(3)
Sig = Sgn(gamma)
If Sig = -1 Then
Gamma = 144000 + Gamma
End If
Alpha = Alpha / 400 : Beta = Beta / 400 : Gamma = Gamma / 400
End Sub
Die Sub Winkel_sortieren durchsucht das Array Winkel(1..3) indirekt nach 0°-Durchgängen in den einzelnen Abständen.
Sollte dieser Umstand auftreten (z.B. in der oberen Abbildung :111360-12400=98960 und damit >72000) wird das Array "geswapt" :
Also steht jetzt im Array:
Winkel(1) =111360 / Winkel(2) =124640 / Winkel(3) =12400
Damit haben wir das ganze so "verdeht" als wenn wir hinter dem Roboter (auf der Gegengeraden) stehen würden und die Baken (gegen den Uhrzeigersinn) in der Reihenfolge C - B - A scannen.
Alpha ist also : 124640 - 111360 = 13280 / 400 = 33,2°
Beta ………… : 12400 - 124640 = -112240 + 144000 = 31760 / 400 = 79,4°
Gamma……... : 111360 - 12400 = 98960 / 400 = 247,4°
Das untere Beispiel könnt Ihr ja mal selbst rechnen !
Fragen hierzu ?
Als nächstes ist nur noch zu klären wann (besser: wo) innerhalb einer Umdrehung am sinnvollsten die Berechnung gestartet werden soll ………………………………….
Liste der Anhänge anzeigen (Anzahl: 1)
und zum Schluß .........
Winkelauswertung wann / wo :
(Winkelauswertung_wo.pdf)
Jetzt müssen wir nur noch festlegen wann die Berechnung innerhalb der Umdrehung starten soll. Wenn der Roboter in unserem Spielfeld z.B. links oben sehr nahe an der Bake A steht ist der Winkel Alpha klein. Die Baken B und C stehen also (vom Roboter aus gesehen) sehr eng zusammen. Wenn wir in diesem Bereich die Berechnung starten (dauert durch die ausgiebige Fließkomma-Bearbeitung doch ein wenig länger) geraten die Variablen (durch zwischenzeitliche Empfangssignale der Bake B) durcheinander.
Besser ist es die Berechnung zu starten wenn gerade nicht viel los ist :
Gamma ist auf dem gesamten Feld >180°. Um ein wenig "Bewegungsfreiheit" während der Fahrt zu haben (ist für die laufende Korrektur des nachfolgend beschriebenen Winkels günstig) liegt dieser auf der Hälfte von Gamma. In diesem Bereich ist normalerweise nicht mit Empfangssignalen zu rechnen.
Damit kann sich der Roboter relativ weit drehen ohne sich gleich wieder selbst zu stören.
Als erstes wird dazu eine Umdrehung in ca. 5° Segmente grob aufgeteilt (144000 / 2048 ~ 70 Segmente). Dies wird u.a. durch Int1 geleistet (vergl. Blockschaltbild/Hardware). Int1_int ist kurz gehalten und beeinflusst durch relativ große zeitliche Abstände die anderen Programmteile nur wenig. Gleichzeitig wird von Int1_int geprüft ob der Winkel "Gamma/2" innerhalb des aktuellen Segmentes liegt. Ist dies der Fall wird die Flagge "Umdrehung_beendet" gesetzt und hiermit dem Hauptprogramm die Freigabe für die endgültige Berechnung der Positionsdaten gegeben.
Code:
Int1_int: 'Auswerte- "Raster"
Berechnen_step = Berechnen_step + 2048
If Berechnen_step >= Ber_winkel_anf And Berechnen_step <= Ber_winkel_end And Umdrehung_beendet = 0 Then
Umdrehung_beendet = 1 'ab hier Auswertung starten
Incr Runde
End If
Return
Mit dem "0" -setzen des Hardwarezählers wird auch Int2 aktiviert:
Code:
Int2_int: 'HW-Zähler wird zurückgesetzt
Berechnen_step = 0
Return
……………………..und damit auch der Segmentzähler "Berechnen_step".
Der "Berechnen-Winkel" bei "Gamma/2":
Zur Verbesserung der Struktur habe ich diesen Teil (wurde weiter oben schon erwähnt) mal in eine Unterroutine gepackt:
Code:
Impuls_pakete: 'alle zusammenhängenden Impulspakete ermitteln
Anz_target_fertig = 1 'Ältere BASCOM-Versionen können nicht mit Index "0" !
If Anz_target > 1 Then
Temp_wa(1) = Wa(1) 'Anfang merken
For N = 1 To Anz_target 'Schleife über alle (Einzel-)Pakete
Diff = Wa(n + 1) - We(n) 'Differenz zwischen den Paketen
If Diff < 0 Then Diff = Diff + 144000 '"0"-Durchgang
If Diff > 0 And Diff < 700 Then '700 ist die Toleranz (~1,7°)
Winkelanz(anz_target_fertig) = Winkelanz(anz_target_fertig) + Winkelanz(n + 1) 'nur Impulse addieren
Else
Wa(anz_target_fertig) = Temp_wa(anz_target_fertig) 'Anfang eintüten
We(anz_target_fertig) = We(n) 'Ende eintüten
Templ1 = We(anz_target_fertig) - Wa(anz_target_fertig) 'Differenz
If Templ1 < 0 Then 'auf "0"-Durchgang prüfen
Templ1 = Templ1 + 144000 'ggf. korrigieren
End If
Templ1 = Templ1 \ 2 'Mitte !
Templ1 = Templ1 + Wa(anz_target_fertig)
If Templ1 > 144000 Then '"0"-Durchgang ?
Templ1 = Templ1 - 144000 'ggf. korrigieren
End If
Winkel(anz_target_fertig) = Templ1 'FERTIG !
Incr Anz_target_fertig 'nächstes Paket
Temp_wa(anz_target_fertig) = Wa(n + 1) 'nächsten Anfang merken
Winkelanz(anz_target_fertig) = Winkelanz(n + 1) 'nächste Impulsanzahl merken
End If
Next N
End If
Decr Anz_target_fertig 's.o.
Return
Die Suche nach: "Winkel(3) + Gamma /2" :
Die Routine Suche_AlBeGa wird immer dann aufgerufen wenn keine Position ermittelt werden kann weil die Anzahl Baken <> 3 ist oder der Roboter eine schnelle Drehung macht (s.o.) und zum Initialisieren. Hier wird keine Position ermittelt, sondern nur die Winkel(1..3) und damit Alpha, Beta und Gamma. Damit das ganze bei z.B. einer 4. Reflexion nicht hängenbleibt gibt es als Abbruchkriterium noch ein Timeout. Es liegt dann vorerst an der übergeordneten Instanz entsprechend zu reagieren. Z.B. könnte ein anderer Standort angefahren werden.
Möglich wäre aber auch eine Plausibilitätskontrolle und damit Ausblendung der "falschen" Bake.
Code:
Suche_AlBeGa:
Timeout = 0
Ber_winkel_anf = Berechnen_step - 2048 'Berechnen_step hat hier zufälligen Wert !
If Ber_winkel_anf < 0 Then
Ber_winkel_anf = Ber_winkel_anf + 144000
End If
Ber_winkel_end = Ber_winkel_anf + 2048 'Fenster knapp vor Berechnen_step
Umdrehung_beendet = 0 'auf ein neues...........
Anz_target = 0
Do
If Umdrehung_beendet = 1 Then
Gosub Impuls_pakete 'alle zusammenhängenden Impulspakete ermitteln
If Anz_target_fertig = 3 then
Call Winkel_sortieren 'Alpha / Beta / Gamma bestimmen
Ber_winkel_anf = Gamma \ 2
Ber_winkel_anf = Ber_winkel_anf * 400
Ber_winkel_anf = Ber_winkel_anf + Winkel(3) 'Berechnung auf der "Gegengeraden"
If Ber_winkel_anf > 144000 Then 'u.U. korrigieren
Ber_winkel_anf = Ber_winkel_anf - 144000
End If
If Ber_winkel_anf >= 141900 Then Ber_winkel_anf = 0 'letztes Segment
Ber_winkel_end = Ber_winkel_anf + 2048
End If
End If
Loop Until Anz_target_fertig = 3 Or Timeout > 2000
Umdrehung_beendet = 0
Anz_target = 0
For N = 1 To 50
Winkelanz(n) = 0
Winkel(n) = 0
Wa(n) = 288000
Temp_wa(n) = 0
We(n) = 0
Next N
If Anz_target_fertig = 3 Then
Lesefehler = 0
Turm_status.0 = 1 'Orientierung vorh.
Turm_status.1 = 0 'KEINE gültigen Daten vorh.
Else
incr Lesefehler
Turm_status.0 = 0 'Kopflos.........
Turm_status.1 = 0 'KEINE gültigen Daten vorh.
Sound Buzzer , 200 , 250
Sound Buzzer , 200 , 350
Sound Buzzer , 200 , 250
Sound Buzzer , 200 , 350
End If
Return
Als nächstes:
Das komplette Programm (TWI-Slave).
Liste der Anhänge anzeigen (Anzahl: 2)
Und nun die gesamte Slave-Software:
(Scanner_106.BAS)
Scanner (Slave) -Register :
(ScannerRegister.zip)
Im Anhang sind die Slave-Register aufgelistet. Die Register 1..9 sind zum Schreiben, die Register 10…19 zum Lesen vorgesehen.
Wichtig ist das schreiben der Parameter AB und BC (Baken-Abstand) nach einem Reset. Ohne diese Werte kommt nur X=0 und Y=0 zurück.
Der Master (Roboter) steuert den Scanner (z.B. für Initialisieren) so an:
- Register 5 (AB) = 480 schreiben 'AB=480cm
- Register 6 (BC) = 560 schreiben 'BC=560cm
- Register 1 schreiben 'Initialisieren (Motor ein / Laser ein / Orientieren)
Code:
Scanner_Initialisierung:
call Scanner_readregister(10 , 0)
if Scanner_Status.3 = 0 then
T_lob = low(ab) 'T_ab>SLAVE
T_hib = high(ab)
call Scanner_writeregister(5 , T_lob , T_hib)
T_lob = low(bc) 'T_bc>SLAVE
T_hib = high(bc)
call Scanner_writeregister(6 , T_lob , T_hib)
end if
if Turm_Status.0 = 0 then
call Scanner_writeregister(2 , 70 , 0) 'Turm Geschw.
waitms 500 ' !!! Sicherheit !!!
call Scanner_writeregister(3 , 1 , 0) 'Laser ein
call Scanner_writeregister(1 , 0 , 0) 'Initialisieren
end if
Return
Nun läuft der Scanner und liefert (hoffentlich) die gewünschten Daten.
Der Master (Roboter) bekommt die Werte nach dem Handshake (z.B. ATMega256 >PCint an PINK.7):
Code:
Isr_pcint2:
If pink.7 = 1 Then
call Scanner_readregister(10 , 0) 'Status
call Scanner_readregister(11 , 0) 'Pos. X
call Scanner_readregister(12 , 0) 'Pos. Y
call Scanner_readregister(14 , 0) 'Winkel2
call Scanner_readregister(16 , 0) 'Baken
call Scanner_readregister(17 , 0) 'Impulse
Winkel_gemessen_ Scanner = Winkel2 / 100
Pcifr.pcif2 = 1
End If
Return
Der Master (Roboter) steuert den Scanner (z.B. für AUS) so an:
Code:
Scanner _ausschalten:
Timeout = 0
do
call Scanner _writeregister(3 , 0 , 0) ' Scanner -Laser AUS
call Scanner _writeregister(2 , 0 , 0) ' Scanner -Motor AUS
call Scanner _readregister(10 , 0) ' Status
if Timeout > 60 then
Print "TIMEOUT Scanner ausschalten !!!"
Print " Scanner _status=" ; Scanner _status
exit do
end if
loop until Scanner _status.5 = 0 and Scanner _status.4 = 0
Return
….und wieder EIN:
Code:
Scanner _einschalten:
Timeout = 0
do
call Scanner_writeregister(2 , 70 , 0) ' Scanner -Motor EIN
waitms 500 ' !!! Sicherheit !!!
call Turm_writeregister(3 , 1 , 0) ' Scanner -Laser EIN
call Turm_readregister(10 , 0)
if Timeout > 60 then
Print "TIMEOUT Scanner einschalten !!!"
Print " Scanner_status=" ; Scanner_status
exit do
end if
loop until Scanner_status.5 = 1 and Scanner_status.4 = 1
Return
Der TWI-Code:
Code:
Sub Scanner_writeregister(byval Befehl as Byte , byval Wert_L as Byte , byval Wert_H as Byte )
I2cstart
I2cwbyte Scanner_slaveid 'D0
I2cwbyte Befehl '1….9
I2cwbyte Wert_L
I2cwbyte Wert_H
I2cstop
End Sub
Code:
Sub Scanner_readregister(byval ScannerRegister as Byte , byval ScannerWert as Byte)
I2cstart
I2cwbyte Scanner_slavewrite 'D0
I2cwbyte ScannerRegister '10……19
I2cwbyte ScannerWert 'für künftige Erweiterungen
I2cstop
I2cstart
I2cwbyte Scanner_slaveread 'D1
I2crbyte Hib , Ack
I2crbyte Lob , Nack
I2cstop
Select case ScannerRegister
case 10
Scanner_Status = Makeint(lob , Hib)
case 11
X_gemessen = Makeint(lob , Hib)
case 12
Y_gemessen = Makeint(lob , Hib)
case 13
Winkel1 = Makeint(lob , Hib)
case 14
Winkel2 = Makeint(lob , Hib)
case 15
Winkel3 = Makeint(lob , Hib)
case 16
Baken = Makeint(lob , Hib)
case 17
Baken_Impulse = Makeint(lob , Hib)
end select
End Sub
Ich hoffe, ich habe nichts vergessen...............:confused:
Damit ist das Projekt (vorerst) abgeschlossen und ich wünsche Euch viel Erfolg damit. :):):)
