IR-Navigation mit TSOP7000, 2 Drehbaken, 15m Reichweite
Hi Gemeinde,
seit ich meinen Rasenrobo in Dienst gestellt habe, habe ich über ein Ortungssystem nachgedacht. Den ursprünglichen Wunsch ihn auf cm genau zu navigieren, um systematisch den Rasen zu mähen, habe ich inzwischen aufgegeben. Für meine 200 m^2 reicht das Zufallssystem. Eine Akkuladung reicht für über 2 h. Danach ist alles niedergemäht. Trotzdem hat mich die Entwicklung eines IR-Navigationssystems gereizt, zumal hier im Roboternetz immer wieder über Navigation mit GPS, US, Bilderverarbeitung diskutiert wird.
Anforderungen waren:
Einfacher Aufbau, niedriger Stromverbrauch, also z.B. kein Laptop für Bildverarbeitung
Einfache Installation, also kein Kamerasystem oder Datentransfer per RF
Keine bewegliche Teile beim Empfänger
Empfänger berechnet seine Position und gibt diese per I2c an den Robo weiter
Aktualisierung der Position im Sekundenbereich.
Herausgekommen ist folgendes:
2 Baken, jeweils
1 Servo
1 IR-LED Osram SFH480-2 (verträgt im Impulsbetrieb bis 2A etwa 5 Grad Abstrahlwinkel)
1 Atmega 32
1 ULN2804L (Darlington-Array)
1 10k (Reset)
ggf. Vorwiderstand für LED. Ohne Vorwiderstand große Reichweite. Falls bei geringeren Entfernungen Störungen durch reflektiertes IR auftreten mit passenden Vorwiderstand Strahlenintensität der IR-LED senken.
1 Empfänger
8 TSOP7000 (arbeiten mit 455KHz)
1 Atmega 32
1 1000uF Elko
(Quarz 16 MHz, funktioniert wahrscheinlich auch mit internen Oszillator)
ggf. Display
Spannungsregler 5V (bei neuen Batterien 4 x 1,5 V spinnt meine Anzeige, ansonsten geht's auch ohne)
1 10k
Die 2 Baken werden in bestimmten Abstand aufgestellt und so ausgerichtet, dass die 0 Grad - Anfangspositionen der Servos auf einer Linie liegen. Im Empfänger-Programm muss nur der Abstand der Servos eingegeben werden. Positionen x und y beziehen sich dann auf die Grundlinie zwischen den beiden Baken.
Zusammengefaßt funktionierts folgendermaßen:
Baken:
Atmega 32 schwenkt die IR-LED hin und her
Das Poti des Servos wird angezapft. Per ADC wird die Stellung des Servos bestimmt.
Am einfachsten ist es sich auf einen Schwenkwinkel von 127 Grad zu beschränken.
Der aktuelle Winkel wir von der IR-LED abgestrahlt
Bake 1 sendet von 1 bis 127, Bake 2 von 128 bis 255
Empfänger:
8 TSOP7000 werden einfach parallel geschaltet und ermöglichen Rundumbeobachtung
gemeinsame Datenleitung geht an Rx des Atmega32
Programm analysiert empfangene Bytes nach Winkel und Kennung und errechnet Position
Entfernung: gut 15 m. Über das Darlingtonarray wäre es möglich weitere IR-LED anzusteuern womit die Reichweite sicherlich weiter erhöht werden könnte.
Genauigkeit pro Bake 1 bis 2 Grad. Macht bei 10 m Entfernung eine Genauigkeit von ungefähr 20 cm.
Die Basic-Programme mit einigen Erläuterungen:
Baken:
Code:
$regfile = "m32def.dat"
$crystal = 8000000
Config Portb = Output
Dim I As Byte , Ii As Byte , Iii As Byte , Iiii As Byte , Imax As Long , Idiv As Long , Imin As Long , I4 As Long , I5 As Byte , I6 As Byte , I7 As Byte , Byt(14) As Byte
Config Portd = Output
Byt(1) = 0 'startbit
Byt(10) = 0 'stopbit
Byt(9) = 1 '0 für Bake A 1 für Bake B
Config Adc = Single , Prescaler = Auto , Reference = Internal
Imin = 700 : Imax = 100 'Startwerte um größten und kleinsten 'Ausschlag des Servos zu bestimmen
Do
For Ii = 1 To 10 'For Next für 10 Bits
'(Start,Winkel(1-128),Bakenkennung,Stop)
If Byt(ii) = 0 Then
I = 0
Do 'do-loop für 14 bursts mit 455 KHz
Toggle Portd.5 ' Übertragung von bit = 1
nop 'IR-LED an Portd.5
nop 'Zahl der nops bestimmt burst-Dauer
nop 'ausprobieren evtl 5 oder 6 besser
nop '4 und 4 nop gut für erste 4 und 3 für zweite Bake
Incr I
Toggle Portd.5
nop 'ausprobieren evtl. 4, 5 oder 6 besser
nop
nop
Loop Until I = 14
Else
I = 0
Do 'Do-loop für gleichlange Pause > bit =0
Toggle Portd.4 'an Portd.4 ist nichts angeschlossen
nop 'Anzahl nops wie oben
nop
nop
nop
Incr I
Toggle Portd.4
nop
nop
nop
End If
Loop Until I = 14
Next
Incr Iii
If Iii = 10 Then 'Alle 10 Bytes wird PWM für Servo erzeugt
Iii = 0
Portd.7 = 1 'Servo an portd.7
Waitms 1 ' Pwm Von 1ms Bis maximal 2ms
For I5 = 0 To I6
Waitus 10
Next
Portd.7 = 0
If I6 = 100 Then I7 = 1
If I6 = 0 Then I7 = 0 'PWM wird langsam erhöht, bzw. erniedrigt
If I7 = 0 Then I6 = I6 + 1 'Incr I6
If I7 = 1 Then I6 = I6 - 1 'Decr I6
Start Adc
I4 = Getadc(0) 'Stellung des Servos wird vom Poti des Servos abgeleitet
Stop Adc
If I4 > Imax And I4 < 1000 Then Imax = I4 'Maximaler ADC -
'Wert wird festgelegt
If I4 < Imin And I4 > 10 Then Imin = I4 'Minimaler ADC Wert wird
'festgelegt
I4 = I4 - Imin : Idiv = Imax : Idiv = Idiv -imin 'IDiv Schankungsbreite
'des ADC-Wertes
If Byt(9) = 0 Then I4 = Idiv -i4
I4 = I4 * 145 : I4 = I4 / Idiv '145 Grad Schwenkbereich des
'Servos (ausmessen und eintragen)
If I4 > 127 Then I4 = 127 'Maximal 127 Grad erlaubt
'128er bit codiert Servo A oder B
Byt(2) = I4 And 1
Byt(3) = I4 And 2
Byt(4) = I4 And 4
Byt(5) = I4 And 8
Byt(6) = I4 And 16
Byt(7) = I4 And 32
Byt(8) = I4 And 64
Else
Waitus 800 'Sendepause zwischen Bytes
End If
Loop
Empfänger:
Code:
$regfile = "m32def.dat"
$crystal = 16000000
Config Lcd = 16 * 2
Config Lcdbus = 4
Config Lcdpin = Pin , Db4 = Porta.2 , Db5 = Porta.3 , Db6 = Porta.4 , Db7 = Porta.5 , E = Porta.1 , Rs = Porta.0
$baud = 10200 'ausprobiert, beste
'Übertragung ggf. anpassen
Echo Off
Dim Empf As Byte
Dim I As Word , I2 As Byte , I3 As Byte , I4 As Byte , I5 As Byte , I6 As Byte , Z As Long , Zalt As Long , S As Long , Erg As Long , Erga As Long , Ergb As Long , Ia As Byte , Ie As Byte , Idiv As Byte, D As Long , Tanx As Single , Tany As Single , Posx As Integer , Posy As Integer
D = 100 'Abstand zwischen den Barken in cm
' Goto Test
Config Serialin = Buffered , Size = 254
Enable Interrupts
Dim Twi_control As Byte 'für I2C Datenübertragung
Dim Twi_status As Byte 'Atmega als Slave eingerichtet
Dim Twi_data As Byte
Declare Sub Twi_init_slave
Twi_data = 0
Call Twi_init_slave
Do
If Ischarwaiting() = 1 Then
I5 = I4 : I4 = I3 : I3 = I2 : I2 = Inkey()
If I5 = I4 And I4 = I3 And I3 = I2 And I2 < 255 And I2 > 0 Then 'mind. 3 aufeinanderfolgende Bytes müssen übereinstimmen
Incr Z : S = S + I2 : Ie = I2 'Z = Zahl durchgehend
'empfangener Bytes ,S Summe der Bytes
If Z = 1 Then Ia = I2 'Ia erstes Byte, Ie letztes Byte einer Serie
End If
End If
Incr I
If I = 10000 And Z > 0 Then 'In kurzen Abständen Kontrolle ob Bytes
' empfangen wurden
If Zalt = Z Then 'Z hat sich kurzzeitig nicht mehr geändert.
'D.h. Serie zu Ende
If Z > 15 Then 'Serie wird ausgewertet wenn mindestens 15
' Bytes in einer Serie sind
If Ia > Ie Then
Idiv = Ia - Ie
Else
Idiv = Ie - Ia
End If
CLS: Erg = S / Z : Locate 1 , 13 : Lcd Erg : Locate 1 , 1 : Lcd Z : Locate 1 , 5 : Lcd Ia , : Locate 1 , 9 : Lcd Ie
If Idiv < 30 then 'erster und letzter gemessener Winkel dürfen
'sich max um 30 unterscheiden
If Erg < 128 Then
Erga = Erg 'Winkel von Bake 1
Else
Ergb = Erg - 128 'Winkel von Bake 2
End If
End If
Tanx = Erga : Tanx = 90 -tanx : Tanx = Deg2rad(tanx) : Tanx = Tan(tanx)
Tany = Ergb : Tany = 90 -tany : Tany = Deg2rad(tany) : Tany = Tan(tany)
Tany = Tanx + Tany : Tany = D / Tany : Posy = Tany 'Koordinaten
'errechnen
Tanx = Tany * Tanx : Posx = Tanx
Locate 2 , 1 : Lcd Posx : Locate 2 , 8 : Lcd Posy :
End If
Z = 0 : S = 0
End If
Zalt = Z : I = 0
End If
' Daten per I2C übermitteln schauen ob TWINT gesetzt ist
Twi_control = Twcr And &H80 ' Bit7 von Controlregister
If Twi_control = &H80 Then
Twi_status = Twsr And &HF8 ' Status
' will der Master ein Byte haben
If Twi_status = &HA8 Or Twi_status = &HB8 Then
Twdr = Low(posx)
End If
Twcr = &B11000100
Waitus 200 If Twi_status = &HA8 Or Twi_status = &HB8 Then
Twdr = High(posx)
End If
Twcr = &B11000100
Waitus 200 If Twi_status = &HA8 Or Twi_status = &HB8 Then
Twdr = Low(posy)
End If
Twcr = &B11000100
Waitus 200
If Twi_status = &HA8 Or Twi_status = &HB8 Then
Twdr = High(posy)
End If
' TWINT muss immer gelöscht werden, damit es auf dem Bus
'weiter geht
Twcr = &B11000100 ' TWINT löschen, mit ACK
End If
Loop
Sub Twi_init_slave
Twsr = 0 ' status und Prescaler auf 0
Twdr = &HFF ' default
Twar = &H68 ' Slaveadresse setzen
Twcr = &B01000100 ' TWI aktivieren, ACK einschalten
End Sub
Test: 'Testprogramm ob TSOP7000 überhaupt reagiert
Config Portd = Input
Do
If Pind.0 = 0 And I2 = 0 Then
Incr Erg : I2 = 1
End If
If Pind.0 = 1 Then I2 = 0
Locate 2 , 6 : Lcd Erg
Loop
Return
End
Soviel für Heute. Am Wochenende gibts Fotos. Batterie ist jetzt leider leer.
Ciao
Christian
Liste der Anhänge anzeigen (Anzahl: 1)
Hi,
der Aufbau ist soweit schon logisch, jeder Servo "Scannt" den Bereich ab, trifft der IR-Strahl (da wäre ein IR-Laser wohl besser) auf den TSOP, weis die RX-Einheit ja welchen Winkel das Servo derzeit besitzt. Aus den beiden Winkel kann er die Position (Dreiecksberechnung) bestimmen.
Aber die Position könnte man auch ohne ADC-Anzapfung herleiten, den die Pulsweite zum stellen der Servos ist proportional zum Stellwinkel.
Im Anhang noch eine kleine Skizze....
Liste der Anhänge anzeigen (Anzahl: 4)
Hallo,
hier ersten Fotos:
gesamtes Ortungssystem:
Bild hier
Fast nichts auf der Platine:
Bild hier
Empfänger:
Bild hier
Bild hier
Das Kabel am Empfänger ist der Anschluss zum I2C-Bus.
Video, allerdings nur von der Bake
[stream:b12e512b89]http://www.rasenrobo.de/Bake/IMPG2282.avi[/stream:b12e512b89]
Weiteres Video folgt hoffentlich heute abend.
Sicherlich könnte man statt der Servos Schrittmotoren nehmen. Übliche Schrittmotore haben aber nur 200 Schritte und eine Auflösung von 1,8 Grad. Wäre einen Versuch wert zu sehen womit man die besseren Ergebnisse erreicht.
@robotnics: Die PWM direkt zu senden liegt natürlich nahe. Aber der Servo hinkt der PWM-Vorgabe immer einige Grad hinterher. Hängt natürlich davon ab wie schnell der Servo dreht. Bei mir ergaben sich deutliche Differenzen in der Winkelangabe nach PWM, je nach dem ob der Servo rechts oder links rum gedreht hat. Greift man die Positon am Poti ab, ist diese Differenz zumindest weg.
Zum IR-Laser: Ein Laser überstreicht den Sensor innerhalb sehr kurzer Zeit. Nimmt man eine Laserstrahl- und Sensorbreite von 5 mm und 10 m Abstand und eine Drehdauer des Servo von etwa 1 sec an, strahlt der Laser nur etwa 100 bis 200 us auf den Sensor. Für einen sicheren Empfang eines einzigen Datenpaketes braucht man aber länger. 455 KHz bedeutet 1 burst dauert 2 us. 14 bursts braucht man um die maximale Empfindlichkeit des TSOP zu erreichen. D.h. es vergehen 28 us pro bit. Mit Laser möchte man natürlich auch eine bessere Auflösung als mit einer IR-LED, sagen wir 512. Hierfür braucht man 9 bit. Mit bit zur Kodierung des Servo , start und stopbit sind´s insgesamt also 12 bit. D.h. Pro Datenpaket vergehen 12 x 28 = bereits 330 us. Dann muss man den Fall bedenken, dass der Laser dummerweise dann den TSOP erreicht, wenn bereits das erste bit gesendet wurde. Zwischen 2 Datenpause braucht man eine Pause. Erst danach kommt evtl. ein vollständig übertragenes Datenpaket zustande. D.h. es braucht fast 1 ms um ein Paket sicher zu übertragen.
Ausserdem sind IR-Laser nicht ungefährlich. Man wird nicht geblendet und hat doch plötzlich nicht nur einen sondern zwei blinde Flecke im Auge. IR-Laser sind deshalb auch nur schwer zu bekommen. Würde ich mich selbst nicht rantrauen und schon gar nicht anderen eine Bauanleitung mit IR-Laser geben.
Zum Problem der Überschneidung der Datenpakete beider Baken beim Empfang: Eine Möglichkeit ist die Servos mit etwas unterschiedlicher Geschwindigkeit drehen zu lassen. Dann kann es nur gelegentlich zu fehlerhaften Datenpaketen kommen. Diese Ausreißer müßte man herausfiltern. Besser ist es die Servos abwechselnd senden zu lassen. D.h. Servo1 sendet z.B. nur beim Rechtsschwenk, Servo2 nur beim Linksschwenk. Beide Servos müssen dann zum gleichen Zeitpunkt gestartet werden. Natürlich verlangsamt sich dann die Aktualisierung der Positionsangeben.
Weiteres Video folgt.
Gruß
Christian
