Programm 4 (Sensobot1 komplett)

$regfile = "m32def.dat"
$framesize = 32
$swstack = 32
$hwstack = 32

Dim Bumper1 As Bit                                          '0 &1
Dim Bumper2 As Bit
Dim Bumper3 As Bit
Dim Bumper4 As Bit
Dim Ldr As Bit
Dim Volt As Single                                          '1,5*10^-45 - 3.4 x 10^38
Dim Bat As Single
Dim Ws As Integer
Dim Cny1 As Integer
Dim Cny2 As Integer
Dim Cny3 As Integer
Dim Ts As Integer
Dim I As Integer
Dim Ton As Integer
Dim Speedli As Integer
Dim Speedre As Integer                                      '-32768 - +32768
Dim Li As Integer
Dim Re As Integer
Dim Taste As Byte                                           '0 - 255
Dim Fahren As Byte
Dim Los As Byte
Dim Fall As Byte
Dim Weiter As Byte
Dim Rueckend As Byte
Dim Koppler1 As Byte
Dim Koppler2 As Byte
Dim Koppler3 As Byte
Dim M1 As Integer                                           '5 Merker - Für diverses
Dim M2 As Integer
Dim M3 As Integer
Dim M4 As Integer
Dim M5 As Integer
Dim W As Integer

Declare Sub 2motvorw()                                      'Subs anmelden
Declare Sub Rechtskurve()
Declare Sub Linkskurve()
Declare Sub Rueckwaerts()
Declare Sub Anhalten()
Declare Sub Showporta()
Declare Sub Sharpana0()
Declare Sub Tastenabfrage()
Declare Sub Motorregelung()
Declare Sub Fahrennachfall()
Declare Sub Textausgeben()
Declare Sub Handysteuerung()
Declare Sub Musik()
Declare Sub Cny()

$crystal = 16000000                                         'Quarzfrequenz
$baud = 9600

Config Adc = Single , Prescaler = Auto                      'Für Tastenabfrage und Spannungsmessung

Config Pina.7 = Input                                       'Für Tastenabfrage
Porta.7 = 1                                                 'Pullup Widerstand ein
Config Pina.0 = Input                                       'Sharp120
Config Pina.1 = Input                                       'CNY1
Porta.1 = 1
Config Pina.2 = Input                                       'CNY2
Porta.2 = 1
Config Pina.3 = Input                                       'CNY3
Porta.3 = 1

Config Pinb.3 = Input                                       '4 Bumper Pins B3,B4,C2,C3,C4
Config Pinb.4 = Input
Config Pinc.2 = Input
Config Pinc.3 = Input
Config Pinc.4 = Input                                       'LDR für Handy Einschaltung

Const Ref = 5 / 1023                                        'Für Batteriespannungsberechnung Faktor

'Motore
'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 = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down
Pwm1a = 0
Pwm1b = 0
Tccr1b = Tccr1b Or &H02                                     'Prescaler = 8
'Ende Motoren Vorbereitungen
M1 = 0 : M2 = 0 : M3 = 0 : M4 = 0 : M5 = 0                  'Merker=0

Handyein:
Call Sharpana0
While Ldr = 1

Call Musik
Call Sharpana0

Wend
Call Handysteuerung
Goto Handyein1

Do
Weiter = 0
Call Tastenabfrage
Call Sharpana0
Handyein1:                                                  'Sharp u. 5 Bumper abfragen
Call Motorregelung
Rem Call Textausgeben
If Taste <> 0 And Fahren = 1 Then
Select Case Taste
Case 1 : Call Showporta                              'Taste 1 Ports anz.
Case 2 : Call Anhalten                               'Taste 2 Mots Stop
Case 3 : Call 2motvorw
Case 4 : Call Rechtskurve
Case 5 : Call Cny                                    'Fahren Mit Linienerkennung
End Select
End If
If Los <> 0 Then
If Fahren <> 0 And Taste = 0 Then                       'Fahre Los
Select Case Fahren                                   'Falls keine Taste gedrückt
Case 1                                               'Und Fahren>0
Call Fahrennachfall

Case 2
Call Motorregelung
Call Fahrennachfall

Case 3
Call Anhalten
Call Rueckwaerts
Call Sharpana0
Call Anhalten

Call Fahrennachfall

End Select
End If
End If
Waitms 10

Loop
End



Sub Showporta()                                             ' Analogwert Port A
Config Porta = Input
For I = 1 To 5                                           ' Pullup Widerstände Ports 1 - 5 Ein
Porta.i = 1                                           ' Port0 kein Pullup wg Sharp
Next I
Print
Print "Ermittelte Messwerte an Port A:"
For I = 0 To 7                                           ' Alle Eingäne messen
Start Adc
Ws = Getadc(i)
Volt = Ws * Ref
Print "Pin " ; I ; " ADC-Wert= " ; Ws ; " bei 5V REF waeren das " ; Volt ; " Volt"
Next I
Ws = Getadc(6)
Volt = Ws * Ref
Bat = Volt * 5.2941
Print "Batteriespannung = " ; Bat ; " Volt"
End Sub

Sub 2motvorw()                                              'Motoren Ein
'Linker Motor ein
Portc.6 = 1                                              'Richtung vorw.
Portc.7 = 0                                              'Richtung vorw.
Portd.4 = 1                                              'Linker Motor EIN

'Rechter Motor ein
Portb.0 = 1                                              'Richtung vorw.
Portb.1 = 0                                              'Richtung vorw.
Portd.5 = 1                                              'rechter Motor Ein

Pwm1a = Speedre
Pwm1b = Speedli
Los = 1
End Sub

Sub Rechtskurve()                                           'Rechtsdrehung
Weiter = Fall
'Linker Motor ein
Portc.6 = 1                                              'Richtung vorw.
Portc.7 = 0                                              'Richtung vorw.
Portd.4 = 1                                              'Linker Motor EIN

'Rechter Motor ein
Portb.0 = 0                                              'Richtung rück.
Portb.1 = 1                                              'Richtung rück.
Portd.5 = 1                                              'rechter Motor Ein

Pwm1a = Speedre
Pwm1b = Speedli

While Fall = Weiter
Call Sharpana0
Waitms 60                                             'Rechtsdrehen XX mS
Wend

Los = 1
End Sub

Sub Linkskurve()                                            'linkssdrehung
Weiter = Fall
'Linker Motor ein
Portc.6 = 0                                              'Richtung rück.
Portc.7 = 1                                              'Richtung rück.
Portd.4 = 1                                              'Linker Motor EIN

'Rechter Motor ein
Portb.0 = 1                                              'Richtung vorw.
Portb.1 = 0                                              'Richtung vorw.
Portd.5 = 1                                              'rechter Motor Ein

Pwm1a = Speedre
Pwm1b = Speedli
While Fall = Weiter
Call Sharpana0
Waitms 60                                             'Linksdrehen XX mS
Wend

Los = 1
End Sub

Sub Anhalten()                                              'Anhalten
'Linker Motor ein
Portc.6 = 1                                              'Richtung stop.
Portc.7 = 1                                              'Richtung stop.
Portd.4 = 1                                              'Linker Motor EIN

'Rechter Motor ein
Portb.0 = 1                                              'Richtung stop.
Portb.1 = 1                                              'Richtung stop.
Portd.5 = 1                                              'rechter Motor Ein

Pwm1a = 1024
Pwm1b = 1024
Waitms 28                                             'Nicht!!!!!! verkleinern Sonst!!!
Los = 0                                                  'Abbremsen XX mS
End Sub

Sub Rueckwaerts()                                           'Rückwärts
Rueckend = 0
Weiter = Fall
'Linker Motor ein
Portc.6 = 0                                              'Richtung vorw.
Portc.7 = 1                                              'Richtung vorw.
Portd.4 = 1                                              'Linker Motor EIN

'Rechter Motor ein
Portb.0 = 0                                              'Richtung vorw.
Portb.1 = 1                                              'Richtung vorw.
Portd.5 = 1                                              'rechter Motor Ein
Pwm1a = Speedre
Pwm1b = Speedli
While Fall = Weiter And Rueckend < 5
Incr Rueckend
Call Sharpana0
Waitms 180                                          'Rückwärts XX mS
Wend

Los = 1
End Sub


Sub Sharpana0()
Bumper1 = Pinb.3                                        '4 Bumper abfragen
Bumper2 = Pinb.4
Bumper3 = Pinc.2
Bumper4 = Pinc.3
Ldr = Pinc.4

If Bumper1 = 0 Then                                     '4 Bumper auswerten
Fall = 1                                             ' ( Fälle )
End If                                                  'Bumper 1 links Seite
If Bumper2 = 0 Then                                     'Bumper 4 rechts Seite
Fall = 2                                             'Bumper 2 links vorne
End If                                                  'Bumper 3 rechts vorne
If Bumper3 = 0 Then                                     'Bumper 5 hinten
Fall = 3                                             'z.Zt. nicht angebaut
End If
If Bumper4 = 0 Then
Fall = 4
End If
If Bumper1 = 0 And Bumper2 = 0 Then
Fall = 5
End If
If Bumper2 = 0 And Bumper3 = 0 Then
Fall = 6
End If
If Bumper3 = 0 And Bumper4 = 0 Then
Fall = 7
End If
If Bumper1 = 0 And Bumper2 = 0 And Bumper3 = 0 Then
Fall = 8
End If
If Bumper2 = 0 And Bumper3 = 0 And Bumper4 = 0 Then
Fall = 9
End If
If Bumper1 = 0 And Bumper2 = 0 And Bumper3 = 0 And Bumper4 = 0 Then
Fall = 10
End If
If Bumper1 = 1 And Bumper2 = 1 And Bumper3 = 1 And Bumper4 = 1 Then
Fall = 11
End If


Fahren = 0                                               ' Sharp abfragen
Start Adc
Ws = Getadc(0)
Cny1 = Getadc(1)
Cny2 = Getadc(2)
Cny3 = Getadc(3)

If Ws > 0 Then

Select Case Ws
Case 0 To 350
Fahren = 1
Case 351 To 510
Fahren = 2
Case 511 To 715
Fahren = 3
End Select
End If
End Sub

Sub Tastenabfrage()                                         'Diese Unterfunktion fragt die Tastatur am analogen Port ab

Taste = 0
Start Adc
Ts = Getadc(7)
'Tastenabfrage anpassen ADC Wert Ws
If Ts < 500 Then
Select Case Ts
Case 400 To 450
Taste = 1
Case 330 To 380
Taste = 2
Case 260 To 305
Taste = 3
Los = 1                                         'Taste 3 gedrückt = Losfahren
Case 180 To 220
Taste = 4
Case 90 To 130
Taste = 5
End Select

End If
End Sub

Sub Motorregelung()

Select Case Fall
Case 1
Speedli = 960                              'Links Schneller
Speedre = 16
Case 2 To 3
Speedli = 920                              'Geradeausfahrgeschwindigkeit
Speedre = 960                              'Rechter Mot Läuft etwas langsamer
Case 4
Speedli = 16                               'Rechts Schneller
Speedre = 960
Case 5
Speedli = 920                              'Geradeausfahrgeschwindigkeit
Speedre = 960                              'Rechter Mot Läuft etwas langsamer
Case 6
Speedli = 960
Speedre = 80
Case 7 To 9
Speedli = 920
Speedre = 960
Case 10
Speedli = 80
Speedre = 960
Case 11
Speedli = 920                              'Geradeausfahrgeschwindigkeit
Speedre = 960                              'Rechter Mot Läuft etwas langsamer
End Select

If Fahren = 2 Or Fahren = 3 Then
If Fall = 2 Or Fall = 3 Or Fall = 5 Or Fall = 7 Or Fall = 8 Or Fall = 9 Then
Speedli = Speedli - 240
Speedre = Speedre - 240
End If
End If
If Fahren = 2 Or Fahren = 3 Then
If Fall = 11 Then
Speedli = 80
Speedre = 960
End If
End If

End Sub

Sub Fahrennachfall()

Einsprung:
If Fall = 2 Or Fall = 5 Or Fall = 8 Then
Call Anhalten
Call Rechtskurve
Call Anhalten
End If
If Fall = 3 Or Fall = 7 Or Fall = 9 Then
Call Anhalten
Call Linkskurve
Call Anhalten
End If
If Fall = 6 Or Fall = 10 Then
Call Anhalten
Call Rueckwaerts
Call Anhalten
Call Sharpana0
Goto Einsprung
End If
Call 2motvorw
End Sub

Sub Textausgeben()                                          'Aktivieren durch Rem entfernen

Print " Fall = " ; Fall
Print " Taste = " ; Taste
Print " Fahren = " ; Fahren
Print " ADC-Wert = " ; Ws
Print " Los = " ; Los
Print " Speedli = " ; Speedli
Print " Speedre = " ; Speedre
Print " Bumper1 = " ; Bumper1
Print " Bumper2 = " ; Bumper2
Print " Bumper3 = " ; Bumper3
Print " Bumper4 = " ; Bumper4
Print " Ldr = " ; Ldr
Print " CNY70 Nr.1 = " ; Cny1
Print " CNY70 Nr.2 = " ; Cny2
Print " CNY70 Nr.3 = " ; Cny3
Print " Koppler1 = " ; Koppler1
Print " Koppler2 = " ; Koppler2
Print " Koppler3 = " ; Koppler3

End Sub

Sub Handysteuerung()

If Ldr = 0 And M1 = 0 Then                              'Display hell, dann fahren
Taste = 3
Incr M1
End If

End Sub

Sub Musik()

M2 = 0
W = 100
While M2 < 6

Sound Portd.7 , 5 , W                                 'SOUND  pin, duration, pulses
W = W + 1
If W > 1000 Then
W = 100
Incr M2
End If
Wend
M2 = 0

End Sub

Sub Cny()

Start Adc
Cny1 = Getadc(1)
Cny2 = Getadc(2)
Cny3 = Getadc(3)

Call Sharpana0
If Fahren = 1 Then                                         'NurFahren Wenn Weg frei

If Cny1 > 0 Then

Select Case Cny1
Case 1 To 400
Koppler1 = 1                                    'Weiss
Case 401 To 500
Koppler1 = 2                                    'Halb Halb
Case 501 To 1024
Koppler1 = 3                                    'Schwarz
End Select
End If

If Cny2 > 0 Then

Select Case Cny2
Case 1 To 400
Koppler2 = 1                                    'Weiss
Case 401 To 500
Koppler2 = 2                                    'Halb Halb
Case 501 To 1024
Koppler2 = 3                                    'Schwarz
End Select
End If

If Cny3 > 0 Then

Select Case Cny3
Case 1 To 400
Koppler3 = 1                                    'Weiss
Case 401 To 500
Koppler3 = 2                                    'Halb Halb
Case 501 To 1024
Koppler3 = 3                                    'Schwarz
End Select
End If
End If
End Sub

 

Die Robotik & Arduino Seite