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
Zitieren
Wollte mir eigentlich auch mal nen paar TSOP7000 zulegen, aber sie sind auf einmal ausverkauft...
/
Berechtigungen
Lesezeichen