Code:
$regfile = "m32def.dat"
$crystal = 16000000 'Quarzfrequenz
$baud = 9600
Sound Portd.7 , 200 , 900 'BEEP
'Servo Config
Config Servos = 1 , Servo1 = Portd.3 , Reload = 10 'Turmservo x
Config Portd = 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 Taste As Byte
Dim A0 As Integer
Dim A1 As Integer
Dim A2 As Integer
Dim A3 As Integer
Dim A2_l As Integer
Dim A2_r As Integer
Dim Ton As Integer
Dim N As Integer
Dim I As Integer
Dim Center As Integer
Dim Servopos As Integer
Dim Left As Integer
Dim Right As Integer
Left = 50
Right = 150
Center = 100
Servopos = Center
' 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 = 10 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down
Pwm1a = 0
Pwm1b = 0
Tccr1b = Tccr1b Or &H02 'Prescaler = 8
Wait 4
Sound Portd.7 , 300 , 200
Wait 1 'BEEP
Sound Portd.7 , 1000 , 500
Sound Portd.7 , 100 , 500 'BEEP
Sound Portd.7 , 100 , 600 'BEEP
Sound Portd.7 , 100 , 700 'BEEP
'### Hier startet Scout-Schleife ################################################################################
Do
Servopos = Center
Servo(1) = Servopos
'Linker Motor ein
Portc.6 = 1 'bestimmt Richtung
Portc.7 = 0 'bestimmt Richtung
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
Pwm1a = 1023
Pwm1b = 1023
'Messung der Abstände
Start Adc
A0 = Getadc(0)
'A1 = Getadc(1)
A2 = Getadc(2)
'If Schleife für Frontal Wand
'********
If A0 > 380 Then 'If Abstand Frontsensor 1 kleiner als 15cm
'## STOP
Pwm1a = 0
Pwm1b = 0
'### BEEEPILEEEP
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 , 400
Sound Portd.7 , 300 , 500
Sound Portd.7 , 300 , 200
Sound Portd.7 , 700 , 800
'Servo kopf drehen, rechts links vermessen.
'##########################################
Servopos = Center
Servo(1) = Servopos
Waitms 250
Servopos = Left 'Servo nach links drehen
Servo(1) = Servopos
Wait 1
A2_r = Getadc(2) 'Sensor links auslesen und A2_l zuweisen
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Waitms 150
For Servopos = Left To Right Step 10
Servo(1) = Servopos
Waitms 10
Next
Wait 1
A2_l = Getadc(2) 'Sensor rechts auslesen und A2_r zuweisen
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
Waitms 150
For Servopos = Right To Left Step -10
Servo(1) = Servopos
Waitms 10
Next
Waitms 250
Servopos = Center
Servo(1) = Servopos
Sound Portd.7 , 300 , 400
Sound Portd.7 , 300 , 500
Sound Portd.7 , 300 , 200
Sound Portd.7 , 700 , 800
'####################################################
If A2_l > A2_r Then 'wenn rechts mehr platz als links dann zurückfahren und nach rechts drehen
'stop
Pwm1a = 0
Pwm1b = 0
Wait 1
'Linker Motor vorwärts
Portc.6 = 1 'bestimmt Richtung
Portc.7 = 0 '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 'rechter Motor EIN
'###########
'nach rechts drehen
Waitms 250
Pwm1a = 1023
Pwm1b = 1023
Wait 1
'stop
Pwm1a = 0
Pwm1b = 0
Waitms 150
End If
'####################################################
If A2_r > A2_l Then 'wenn links mehr platz als rechts dann zurückfahren und nach links drehen
'stop
Pwm1a = 0
Pwm1b = 0
Waitms 250
'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
Waitms 250
'###########
'nach links drehen
Pwm1a = 1023
Pwm1b = 1023
Wait 1
'stop
Pwm1a = 0
Pwm1b = 0
Waitms 150
End If
'####################################################
End If
Loop
Lesezeichen