Software Parabola
Uit RobotMC.be
'################################################### 'RoboterNetz Board RN-CONTROL 1.1 'Autor: Frank 'Weitere Beispiele und Beschreibung der Hardware 'unter http://www.Roboternetz.de oder robotikhardware.de 'Koen Van den Heuvel 'Toepassing op Mini-Sumo Parabola '####################################################### Declare Sub Motortest() Declare Sub Domotor() Declare Function Tastenabfrage() As Byte $regfile = "m32def.dat" Dim I As Integer Dim J As Integer Dim N As Integer Dim Ton As Integer $crystal = 7372800 'Quarzfrequenz $baud = 19200 Config Adc = Single , Prescaler = Auto 'Für Tastenabfrage und Spannungsmessung Config Pina.7 = Input 'Für Tastenabfrage Porta.7 = 1 'Pullup Widerstand ein Const Ref = 5 / 1023 'Für Batteriespannungsberechnung Dim Taste As Byte Dim Volt As Single ' Für Motorentest '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 Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down Pwm1a = 0 Pwm1b = 0 Tccr1b = Tccr1b Or &H02 'Prescaler = 8 Dim Motorleft As Integer Dim Motorright As Integer Motorleft = 0 Motorright = 0 Dim Sharpleft As Integer Dim Sharpright As Integer Dim Sharpdifference As Integer Dim Floorleft As Integer Dim Floorright As Integer Dim Direction As Integer Direction = 0 Dim Running As Bit Running = 0 I = 0 'Sound Portd.7 , 400 , 450 'BEEP 'Sound Portd.7 , 400 , 250 'BEEP 'Sound Portd.7 , 400 , 450 'BEEP Print Print "**** RN-CONTROL 1.1 *****" Print "Sumo" Print Do Taste = Tastenabfrage() If Taste <> 0 Then Select Case Taste Case 1 Running = 1 Case 2 Running = 0 Case 3 '5 seconds countdown Config Portc = Output For I = 1 To 5 'looplicht For J = 0 To 7 Portc.j = 0 Waitms 40 Portc.j = 1 Next J For J = 7 To 0 Step -1 Portc.j = 0 Waitms 40 Portc.j = 1 Next J N = I * 50 N = 1000 - N Sound Portd.7 , 200 , N 'BEEP Next I Config Portc = Output Running = 1 Case 4 Motorleft = -255 Motorright = 255 Case 5 Motorleft = 0 Motorright = 0 End Select End If Sharpleft = Getadc(0) Sharpright = Getadc(1) Floorleft = Getadc(3) Floorright = Getadc(2) Sharpdifference = Sharpleft - Sharpright Sharpdifference = Abs(sharpdifference) Sharpdifference = Sharpdifference / 2 If Sharpdifference > 255 Then Sharpdifference = 255 End If If Sharpleft < Sharpright Then Motorleft = 255 Motorright = 255 - Sharpdifference Else Motorleft = 255 - Sharpdifference Motorright = 255 End If ' enkel reageren als > 120 (60cm) If Sharpleft < 120 Then If Sharpright < 120 Then ' niets te zien rondkijken in de richting waar laatst iets gezien If Direction = 0 Then Motorleft = 255 Motorright = -255 End If If Direction = 1 Then Motorleft = -255 Motorright = 255 End If End If End If ' vloer sensoren If Floorleft < 700 Then If Running = 1 Then Motorleft = -255 Motorright = 255 Else Motorleft = 0 Motorright = 0 End If Call Domotor Waitms 500 End If If Floorright < 700 Then If Running = 1 Then Motorleft = 255 Motorright = -255 Else Motorleft = 0 Motorright = 0 End If Call Domotor Waitms 500 End If If Motorright < Motorleft Then Direction = 0 End If If Motorright > Motorleft Then Direction = 1 End If If Running = 1 Then Call Domotor Else Motorleft = 0 Motorright = 0 Call Domotor End If Waitms 20 Print "SL: " ; Sharpleft ; " SR: " ; Sharpright ; " SD: " ; Sharpdifference ; " FL: " ; Floorleft ; " FR: " ; Floorright Loop End 'Diese Unterfunktion fragt die Tastatur am analogen Port ab Function Tastenabfrage() As Byte Local Ws As Word Tastenabfrage = 0 Ton = 600 Start Adc Ws = Getadc(7) If Ws < 1010 Then Select Case Ws Case 410 To 450 Tastenabfrage = 1 Ton = 550 Case 340 To 380 Tastenabfrage = 2 Ton = 500 Case 265 To 305 Tastenabfrage = 3 Ton = 450 Case 180 To 220 Tastenabfrage = 4 Ton = 400 Case 100 To 130 Tastenabfrage = 5 Ton = 350 End Select 'Sound Portd.7 , 400 , Ton 'BEEP End If End Function Sub Domotor() Portd.4 = 1 'Linker Motor EIN Portd.5 = 1 'rechter Motor EIN 'Linker Motor If Motorleft > 0 Then Portc.6 = 1 Portc.7 = 0 Pwm1b = Motorleft Else Portc.6 = 0 Portc.7 = 1 Pwm1b = -motorleft End If 'Rechter Motor If Motorright > 0 Then Portb.0 = 1 Portb.1 = 0 Pwm1a = Motorright Else Portb.0 = 0 Portb.1 = 1 Pwm1a = -motorright End If End Sub