Code:
$regfile = "m32def.dat"
$crystal = 16000000 'Quarzfrequenz
$baud = 9600
'Timer
Config Timer2 = Timer , Prescale = 1024
On Timer2 Timer_irq
Const Timervorgabe = 100
Enable Timer2
Config Lcd = 16 * 2
Config Lcdpin = Pin , Db4 = Portc.0 , Db5 = Portc.1 , Db6 = Portc.4 , Db7 = Portc.2 , E = Portc.3 , Rs = Portb.4
Config Lcdbus = 4
'Servo Config
Config Servos = 2 , Servo1 = Portd.3 , Servo2 = Portb.3 , Reload = 10 'Turmservo x
Config Portd = Output
Config Pinb.3 = Output
Enable Interrupts
'Ende Servo Config
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 Volt As Single
Dim D As Integer
Dim I As Byte
Dim L As Integer
Dim H As Integer
Dim K As Integer
Dim M As Integer
Dim N As Integer
Dim S As Integer
Dim A0 As Integer
Dim A1 As Integer
Dim A2 As Integer
Dim A3 As Integer
Dim A2_l As Integer
Dim A2_l1 As Integer
Dim A2_l2 As Integer
Dim A2_l3 As Integer
Dim A2_lm As Integer
Dim A2diff1 As Integer
Dim A2diff2 As Integer
Dim A2_r As Integer
Dim A2_r1 As Integer
Dim A2_r2 As Integer
Dim A2_r3 As Integer
Dim A2_rm As Integer
Dim A2_wd As Integer
Dim Groesser As Integer
Dim Pos As Integer
Dim X As Integer
Dim Y As Integer
'-------------------------
Dim Center As Integer
Dim Left As Integer
Dim Right As Integer
Dim Leftmax As Integer
Dim Rightmax As Integer
Dim Right1 As Integer
Dim Right2 As Integer
Dim Right3 As Integer
Dim Left1 As Integer
Dim Left2 As Integer
Dim Left3 As Integer
Left = 130
Right = 70
Left1 = 135
Left2 = 150
Left3 = 165
Right1 = 65
Right2 = 50
Right3 = 35
Leftmax = 180
Rightmax = 20
Center = 100
S = 0
Servo(1) = Center
'-------------------------
Dim Center2 As Integer
Dim Oben As Integer
Dim Unten As Integer
Dim Obenmax As Integer
Dim Untenmax As Integer
Oben = 90
Unten = 110
Obenmax = 70
Untenmax = 130
Servo(2) = Center
'String auf Display anzeigen
'-------------------------
'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
Sound Portd.7 , 200 , 400
Wait 3
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "init..."
Sound Portd.7 , 200 , 400
Sound Portd.7 , 200 , 400
Wait 1
Cls 'loesche das LCD Display
Locate 1 , 3 'Cursor auf 1 Zeile, 1 Spalte
Lcd "start..." 'String auf Display anzeigen
L = 100 'beschleunigen
Do
Servo(1) = L
Waitms 20
L = L + 1
Loop Until L > 170
'Motoren Einschalten!!!
Portb.0 = 1
Portb.1 = 0
Portd.5 = 1
Portc.6 = 0
Portc.7 = 1
Portd.4 = 1
Pwm1a = 600
Pwm1b = 600
L = 170 'beschleunigen
Do
Servo(1) = L
Waitms 20
L = L - 1
Loop Until L < 30
Pwm1a = 0
Pwm1b = 0
Servo(1) = 100
'------------------
Wait 1
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "Starte"
Locate 2 , 1
Lcd "programm"
Servo(2) = 100
Sound Portd.7 , 200 , 400
Wait 1
'Motoren Einschalten!!!
Portb.0 = 1
Portb.1 = 0
Portd.5 = 1
Portc.6 = 1
Portc.7 = 0
Portd.4 = 1
'###################################
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "Go"
L = 0 'beschleunigen
Do
Pwm1a = L
Pwm1b = L
Waitms 30
L = L + 50
Loop Until L > 1023
'Scout Routine *#*#*#*#*#*#
'*#*#*#*#*#*#*#*#*#*#*#*#*#*#*#*#*#*#
N = 0
Do
Servo(1) = Center
Servo(2) = Center
Anfang:
N = N + 1
'Messung der Abstände:
Start Adc
A0 = Getadc(0)
A1 = Getadc(1)
A2 = Getadc(2)
D = Getadc(6)
Volt = D * Ref
Volt = Volt * 5.2941
If Volt < 5.8 Then Gosub Akkuwarnung
If A0 > A1 Then
If A0 > 255 Then
If A0 < 290 Then Gosub Rechtslenken
End If
End If
If A1 > A0 Then
If A1 > 255 Then
If A1 < 290 Then Gosub Linkslenken
End If
End If
If A0 > 290 Or A1 > 290 Or A2 > 340 Then Gosub Ausweichen
If N > 1000 Then Gosub Kameradrehung1
If M = 2 Then Gosub Kameradrehung2
If M = 3 Then
Servo(1) = 100
M = 0
End If
If S > 15000 Then Gosub Idlemode
M = 0
H = 0
'Motoren Einschalten!!!
Portb.0 = 1
Portb.1 = 0
Portd.5 = 1
Portc.6 = 1
Portc.7 = 0
Portd.4 = 1
Pwm1a = 1023
Pwm1b = 1023
Loop
'++++++++++++++ S U B s ++++++++++++++++++++++++++++++++++
'|||||||||||||||||||||||
'VVVVVVVVVVVVVVVVVVVVVVV
Akkuwarnung:
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "Volt="
Locate 2 , 1
Lcd Volt 'String auf Display anzeigen
Do
Pwm1a = L
Pwm1b = L
Waitms 40
L = L - 10
Loop Until L < 1
Pwm1a = 0 'Linker Motor aus
Pwm1b = 0 'rechter Motor aus
'****** S O S **********
Sound Portd.7 , 200 , 400
Waitms 100
Sound Portd.7 , 200 , 400
Waitms 100
Sound Portd.7 , 200 , 400
Waitms 100
Sound Portd.7 , 1000 , 400
Waitms 200
Sound Portd.7 , 1000 , 400
Waitms 200
Sound Portd.7 , 1000 , 400
Waitms 200
Sound Portd.7 , 200 , 400
Waitms 100
Sound Portd.7 , 200 , 400
Waitms 100
Sound Portd.7 , 200 , 400
Start Adc
A0 = Getadc(0)
A1 = Getadc(1)
A2 = Getadc(2)
Servo(2) = 100
L = 100 'beschleunigen
Do
Servo(2) = L
Waitms 30
L = L + 1
Loop Until L > 130
Goto Ende
'Soll sich noch ausrichten damit er weit sehen kann
'
'
'
Return
'************* AUSWEICHEN ******************
Ausweichen:
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "Hindernis"
Locate 2 , 1
Lcd "erkannt"
'## STOP
Pwm1a = 0
Pwm1b = 0
H = H + 1 'Dead Lock Zähler
If H > 5 Then Goto Deadlock
Servo(1) = Right
Waitms 150
'Servo kopf drehen, rechts links vermessen.
A2_r = Getadc(2) 'Sensor links auslesen und A2_l zuweisen
Servo(1) = Left
Waitms 250
'Servo kopf drehen, rechts links vermessen.
A2_l = Getadc(2)
Waitms 150
Servo(1) = 100
'Sensordatenauswertung
'-------------------------------------------
If A2_r > A2_l Then 'wenn links mehr platz als rechts
If A0 > A1 Then ' Dann Zurückfahren Und Nach Links Drehen
'stop
Pwm1a = 0
Pwm1b = 0
'Linker Motor rückwärts
Portc.6 = 0 'bestimmt Richtung
Portc.7 = 1 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'Rechter Motor vorwärts
Portb.0 = 1 'bestimmt Richtung rechter Motor
Portb.1 = 0 'bestimmt Richtung rechter Motor
Portd.5 = 1 'rechter Motor EIN
'###########
'nach links drehen
Pwm1a = 1023
Pwm1b = 1023
Waitms 350
'stop
Pwm1a = 0
Pwm1b = 0
End If
'<<<<<< >>>>>>>>>>> >>>>>>>>>> <<<<<<<<<<<<<<
If A1 > A0 Then ' Dann Zurückfahren Und weit Nach links Drehen
'stop
Pwm1a = 0
Pwm1b = 0
'Linker Motor rückwärts
Portc.6 = 0 'bestimmt Richtung
Portc.7 = 1 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'Rechter Motor vorwärts
Portb.0 = 1 'bestimmt Richtung rechter Motor
Portb.1 = 0 'bestimmt Richtung rechter Motor
Portd.5 = 1 'rechter Motor EIN
'###########
'nach links drehen
Pwm1a = 1023
Pwm1b = 1023
Waitms 750
'stop
Pwm1a = 0
Pwm1b = 0
End If
Cls
End If
'------------------------------
If A2_l > A2_r Then 'wenn rechts mehr platz als links
If A1 > A0 Then 'dann zurückfahren und nach rechts drehen
'stop
Pwm1a = 0
Pwm1b = 0
Servo(2) = Unten
'Rechter Motor rückwärts
Portb.0 = 0 'bestimmt Richtung rechter Motor
Portb.1 = 1 'bestimmt Richtung rechter Motor
Portd.5 = 1 'rechter Motor EIN
'Linker Motor vorwärts
Portc.6 = 1 'bestimmt Richtung
Portc.7 = 0 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'###########
'nach rechts drehen
Pwm1a = 1023
Pwm1b = 1023
Waitms 550
'stop
Pwm1a = 0
Pwm1b = 0
End If
'<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>< <<<<<<<<<<<< >>>>>>>>>>>>>
If A0 > A1 Then 'dann zurückfahren und weit nach rechts drehen
'stop
Pwm1a = 0
Pwm1b = 0
Servo(2) = Unten
'Rechter Motor rückwärts
Portb.0 = 0 'bestimmt Richtung rechter Motor
Portb.1 = 1 'bestimmt Richtung rechter Motor
Portd.5 = 1 'rechter Motor EIN
'Linker Motor vorwärts
Portc.6 = 1 'bestimmt Richtung
Portc.7 = 0 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'###########
'nach rechts drehen
Pwm1a = 1023
Pwm1b = 1023
Waitms 850
'stop
Pwm1a = 0
Pwm1b = 0
End If
End If
Goto Anfang
Return
'************ Rechtslenken ****************
Rechtslenken:
H = H + 1 'Dead Lock Zähler
If H > 5 Then Goto Deadlock
Servo(1) = 110
Servo(2) = Oben
Pwm1a = 1023
Pwm1b = 300
Waitms 250
Pwm1a = 1023
Pwm1b = 1023
Servo(1) = 100
Goto Anfang
Return
'************ linkslenken ****************
Linkslenken:
H = H + 1 'Dead Lock Zähler
If H > 5 Then Goto Deadlock
Servo(1) = 90
Servo(2) = Oben
Pwm1b = 1023
Pwm1a = 300
Waitms 250
Pwm1a = 1023
Pwm1b = 1023
Servo(1) = 100
Goto Anfang
Return
'************ Abstand ****************
Abstand:
M = 1
Goto Anfang
Return
'*****************
Kameradrehung1:
Pwm1a = 1023
Pwm1b = 1023
X = 100
Do
Servo(1) = X
Waitms 20
X = X + 5
Loop Until X > 130
N = 0
M = 2
Goto Anfang
Return
'**********************
Kameradrehung2:
Pwm1a = 1023
Pwm1b = 1023
X = 130
Do
Servo(1) = X
Waitms 20
X = X - 5
Loop Until X < 70
X = 100
M = 3
Goto Anfang
Return
Timer_irq:
Timer2 = Timervorgabe
S = S + 1
Return
Idlemode:
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "Ausrichtung"
Locate 2 , 1
Lcd "gestartet"
Pwm1a = 0
Pwm1b = 0
Servo(2) = 90
Servo(1) = Right3
Waitms 450
A2_r3 = Getadc(2) 'Sensor links auslesen und A2_l zuweisen
Servo(1) = Right2
Waitms 350
A2_r2 = Getadc(2)
Servo(1) = Right1
Waitms 350
A2_r1 = Getadc(2)
Servo(1) = Center
Waitms 350
A2 = Getadc(2)
Servo(1) = Left1
Waitms 350
A2_l1 = Getadc(2)
Servo(1) = Left2
Waitms 350
A2_l2 = Getadc(2)
Servo(1) = Left3
Waitms 350
A2_l3 = Getadc(2)
'Größten Wert suchen - mit 2 Werte vergleichen und kleineren ignorieren
Y = 0
If A2_r3 > A2_r2 Then
Groesser = Right3
Pos = 800
Y = 1
End If
If A2_r2 > Groesser Then
Groesser = A2_r2
Pos = 500
Y = 1
End If
If A2_r1 > Groesser Then
Groesser = A2_r1
Pos = 250
Y = 1
End If
If A2 > Groesser Then
Groesser = A2
Pos = 0
Y = 1
End If
If A2_l1 > Groesser Then
Groesser = A2_l1
Pos = 250
Y = 2
End If
If A2_l2 > Groesser Then
Groesser = A2_l2
Pos = 500
Y = 2
End If
If A2_l3 > Groesser Then
Groesser = A2_l3
Pos = 800
Y = 2
End If
If Y = 1 Then
'Rechter Motor rückwärts
Portb.0 = 1 'bestimmt Richtung rechter Motor
Portb.1 = 0 'bestimmt Richtung rechter Motor
Portd.5 = 1 'rechter Motor EIN
'Linker Motor vorwärts
Portc.6 = 0 'bestimmt Richtung
Portc.7 = 1 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'###########
'nach rechts drehen
Pwm1a = 1023
Pwm1b = 1023
Waitms Pos
'stop
Pwm1a = 0
Pwm1b = 0
End If
If Y = 2 Then
'Rechter Motor rückwärts
Portb.0 = 0 'bestimmt Richtung rechter Motor
Portb.1 = 1 'bestimmt Richtung rechter Motor
Portd.5 = 1 'rechter Motor EIN
'Linker Motor vorwärts
Portc.6 = 1 'bestimmt Richtung
Portc.7 = 0 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'###########
'nach rechts drehen
Pwm1a = 1023
Pwm1b = 1023
Waitms Pos
'stop
Pwm1a = 0
Pwm1b = 0
End If
Servo(1) = 100
Servo(2) = 70
Wait 1
A2 = Getadc(2)
S = 0
Do
A2diff1 = Getadc(2)
Waitms 500
A2diff2 = Getadc(2)
A2_wd = A2diff2 - A2diff1
If A2_wd > 30 Then
S = 32000
End If
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd A2diff1
Loop Until S > 30000
S = 0
Servo(1) = 100
Servo(2) = 100
Goto Anfang
Return
Deadlock:
Cls 'loesche das LCD Display
Locate 1 , 1 'Cursor auf 1 Zeile, 1 Spalte
Lcd "Deadlock"
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
'Linker Motor rückwärts
Portc.6 = 0 'bestimmt Richtung
Portc.7 = 1 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'Rechter Motor rückwärts
Portb.0 = 0 'bestimmt Richtung rechter Motor
Portb.1 = 1 'bestimmt Richtung rechter Motor
Portd.5 = 1
Pwm1a = 900
Pwm1b = 900
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Sound Portd.7 , 300 , 200
Waitms 450
Pwm1a = 0
Pwm1b = 0
'Linker Motor rückwärts
Portc.6 = 0 'bestimmt Richtung
Portc.7 = 1 'bestimmt Richtung
Portd.4 = 1 'Linker Motor EIN
'Rechter Motor rückwärts
Portb.0 = 1 'bestimmt Richtung rechter Motor
Portb.1 = 0 'bestimmt Richtung rechter Motor
Portd.5 = 1
Pwm1a = 700
Pwm1b = 700
Waitms 250
Pwm1a = 0
Pwm1b = 0
Goto Anfang
Return
Ende:
Zitieren
Lesezeichen