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. :):):)
