Hi
mein Robbi macht seine erste Fahrt durchs Wohnzimmer. Er ist mit dem RNBFRA1.2, 2 Getriebemotoren und einem Sharp Entfernungssensor ausgestattet, der durch ein Servo auf 11 verschiedene Positionen zur Messung gebracht wird. Also so eine Art Radar. Momentan fährt er einfach geradeaus und weicht bei einem Hindernis nach rechts aus. Mein Problem ist jetzt, das alles sequentiell abläuft, also die nächste Servoposition zur nächsten Entfernungsmessung erst gemacht wird, nachdem er mit dem Ausweichen fertig ist. Ich würde das Programm also gerne "parallelisieren". Kann mir jemand weiterhelfen? An welcher Stelle müsste man eine Interruptsteuerung einbauen?
Über etwas Hilfe würde ich mich sehr freuen
Gruß
daiyama
Code:$regfile = "m32def.dat" Const Posdelta = 25 '11 Schritte von 0° - 180° Declare Sub Rnb_servob(byval Servonummer As Byte , Byval Position As Byte) Declare Sub Scandistance Declare Sub Nextscanpos Declare Sub Initscanpos Declare Sub Forward Declare Sub Backward Declare Sub Leftdir Declare Sub Rightdir Declare Sub Halt Dim I As Word Dim Arraywert As Byte Dim Scanpos As Byte 'aktuelle Messposition Dim Scandir As Bit 'Rechts herum = 1, Links herum = 0 Dim Scanpositions(11) As Byte 'Array mit Winkelpositionen Dim Port As Byte Dim Distance As Single 'Gemessene Entfernung Dim Delta As Single 'Korrekturwert Dim Adcoutput As Integer 'Ausgabewert des Sensors Dim Vleft As Word 'V linker Motor Dim Vright As Word 'V rechter Motor Const B = 16.025 'Offset der Kurve GP2D12 Const A = 5233.5 'Steigung der Kurve GP2D12 Const Emin = 500 Const Emax = 80 Const Gmax = 400 'Max Geschwindigkeit für Geradeausfahrt Const Kmax = 200 'Max Geschwindigkeit für Kurvenfahrt $crystal = 8000000 $baud = 9600 Config Adc = Single , Prescaler = Auto 'Ports für linken Motor Config Pinc.6 = Output 'Linker Motor Kanal 1 Config Pinc.7 = Output 'Linker Motor Kanal 2 Config Pind.4 = Output 'Linker Motor PWM 'Ports für rechten Motor Config Pinb.0 = Output 'Rechter Motor Kanal 1 Config Pinb.1 = Output 'Rechter Motor Kanal 2 Config Pind.5 = Output 'Rechter Motor PWM Config Scl = Portc.0 'Ports fuer IIC-Bus Config Sda = Portc.1 Enable Interrupts 'Interrupts aktivieren Enable Adc Start Adc 'Die ADC's starten I2cinit I2cstart I2cwbyte &H74 'Schreibbefehl an PCF3 schicken ' Led´s ein ,Motorendstufen ein, Port-Peripherie ein, RBN-Bus Sleep Modus aus (also Peripherie aktiv) I2cwbyte &B00000010 'Datenbyte an PCF3 I2cstop Config Timer1 = Pwm , Pwm = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down Tccr1b = Tccr1b Or &H02 'Prescaler = 8 Call Initscanpos 'Array mit Winkelpos. füllen... Call Forward 'Vorwärts fahren Do Arraywert = ScanPositions(scanpos) Call Rnb_servob(1 , Arraywert) 'Sensor-Servo auf Position bringen Call Scandistance 'Entfernung messen If Distance < 30 Then 'Hindernis in 30 cm... Call Halt 'anhalten Call Backward 'ein Stück zurück... Call Leftdir 'etwas links drehen Call Halt 'anhalten Call Forward 'weiter fahren... End If Call Nextscanpos 'nächste Servopos. Waitms 100 Loop Sub Forward Vleft = 0 Vright = 0 'Linker Motor ein Portc.6 = 0 'bestimmt Richtung linker Motor Portc.7 = 1 'bestimmt Richtung linker Motor Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 1 'bestimmt Richtung rechter Motor Portb.1 = 0 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN I = 0 'Langsam anfahren... Do Vleft = I Vright = I Pwm1a = Vright Pwm1b = Vleft Waitms 50 I = I + 5 Loop Until I > Gmax End Sub Sub Backward Vleft = 0 Vright = 0 'Linker Motor ein Portc.6 = 1 'bestimmt Richtung linker Motor Portc.7 = 0 'bestimmt Richtung linker Motor Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 0 'bestimmt Richtung rechter Motor Portb.1 = 1 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN I = 0 'Langsam anfahren... Do Vleft = I Vright = I Pwm1a = Vright Pwm1b = Vleft Waitms 50 I = I + 10 Loop Until I > Gmax End Sub Sub Leftdir Vleft = 0 Vright = 0 'Linker Motor ein Portc.6 = 0 'bestimmt Richtung linker Motor Portc.7 = 1 'bestimmt Richtung linker Motor Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 0 'bestimmt Richtung rechter Motor Portb.1 = 1 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN I = 0 'Langsam anfahren... Do Vleft = I Vright = I Pwm1a = Vright Pwm1b = Vleft Waitms 50 I = I + 10 Loop Until I > Kmax End Sub Sub Rightdir Vleft = 0 Vright = 0 'Linker Motor ein Portc.6 = 1 'bestimmt Richtung linker Motor Portc.7 = 0 'bestimmt Richtung linker Motor Portd.4 = 1 'Linker Motor EIN 'Rechter Motor ein Portb.0 = 1 'bestimmt Richtung rechter Motor Portb.1 = 0 'bestimmt Richtung rechter Motor Portd.5 = 1 'rechter Motor EIN I = 0 'Langsam anfahren... Do Vleft = I Vright = I Pwm1a = Vright Pwm1b = Vleft Waitms 50 I = I + 10 Loop Until I > Kmax End Sub Sub Halt Pwm1a = 0 'Linker Motor aus Pwm1b = 0 'rechter Motor aus End Sub Sub ScanDistance Adcoutput = Getadc(0) If Adcoutput > Emin Then Adcoutput = Emin End If If Adcoutput < Emax Then Adcoutput = Emax End If Delta = Adcoutput - B Distance = A / Delta Distance = Round(Distance) End Sub Sub Initscanpos ScanPositions(1) = 3 For Scanpos = 2 To 11 ScanPositions(scanpos) = ScanPositions(scanpos - 1) + Posdelta Next Scanpos Scanpos = 1 'Position für erste Messung ScanDir = 1 'Rechts herum For I = 1 To 50 'Bildschirm leer machen Print Next I For I = 1 To 11 Next I End Sub Sub Nextscanpos If ScanDir = 1 Then 'ScanDir = 1 If Scanpos < 11 Then Scanpos = Scanpos + 1 Else ScanDir = 0 'ScanDir umkehren Scanpos = Scanpos - 1 End If Else 'ScanDir = 0 If Scanpos > 1 Then Scanpos = Scanpos - 1 Else ScanDir = 1 'ScanDir umkehren Scanpos = Scanpos + 1 End If End If End Sub Sub Rnb_servob(byval Servonummer As Byte , Byval Position As Byte) Open "comd.7:9600,8,n,1" For Output As #2 Print #2 , "#s" ; Chr(servonummer) ; Chr(position) Close #2 End Sub End>
Zitieren
Lesezeichen