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
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.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
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.)
Zitieren
Lesezeichen