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:
……………………..und damit auch der Segmentzähler "Berechnen_step".Code:Int2_int: 'HW-Zähler wird zurückgesetzt Berechnen_step = 0 Return
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).
Zitieren
Lesezeichen