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