Software Parabola

Uit RobotMC.be
Ga naar: navigatie, zoeken
'###################################################
'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