Code:
$regfile = "m32def.dat"
$crystal = 16000000 'Quarzfrequenz
$baud = 9600
Sound Portd.7 , 200 , 900 'BEEP
'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 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 Center As Integer
Dim Servopos As Integer
Dim Left As Integer
Dim Right As Integer
Left = 50
Right = 150
Center = 100
Servopos = Center
Servo(1) = Servopos
Dim Center2 As Integer
Dim Servopos2 As Integer
Dim Oben As Integer
Dim Unten As Integer
Oben = 80
Unten = 120
Center2 = 100
Servopos2 = Center2
Servo(2) = Servopos2
' 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
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
Servo(2) = Center2
'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
Pwm1b = 1023
Pwm1a = 1023
'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 < 6 Then
Do
Pwm1a = 0
Pwm1b = 0
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
D = Getadc(6)
Volt = D * Ref
Volt = Volt * 5.2941
Wait 3
For I = 100 To 150
Servo(1) = I
Waitms 100
Next
For I = 100 To 120
Servo(2) = I
Waitms 100
Next
For I = 150 To 50
Servo(1) = I
Waitms 100
Next
For I = 120 To 80
Servo(2) = I
Waitms 100
Next
For I = 50 To 100
Servo(1) = I
Waitms 100
Next
For I = 80 To 100
Servo(2) = I
Waitms 100
Next
Loop
End If
'nur ausweich-algorithmus während der Fahrt!
If A1 > A0 Then
If A1 > 250 Then
If A1 < 290 Then
Servo(1) = 70
Servo(2) = Oben
Pwm1b = 1010
Pwm1a = 350
Waitms 300
Pwm1a = 1023
Pwm1b = 1023
Servo(1) = 100
End If
End If
End If
'##################################
If A0 > A1 Then
If A0 > 250 Then
If A0 < 290 Then
Servo(1) = 130
Pwm1a = 1010
Pwm1b = 350
Waitms 300
Pwm1a = 1023
Pwm1b = 1023
Servo(1) = 100
End If
End If
End If
'###############################
'If Schleife für Frontal Wand
'********
If A0 > 290 Or A1 > 290 Or A2 > 350 Then
'## STOP
Pwm1a = 0
Pwm1b = 0
Servo(2) = Oben
'### BEEEPILEEEP
Sound Portd.7 , 300 , 200
Sound Portd.7 , 300 , 800
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
Waitms 250
A2_r = Getadc(2) 'Sensor links auslesen und A2_l zuweisen
'Sound Portd.7 , 300 , 200
'Sound Portd.7 , 300 , 800
Delay
Servopos = Right 'Servo nach links drehen
Servo(1) = Servopos
Waitms 450
A2_l = Getadc(2) 'Sensor rechts auslesen und A2_r zuweisen
' Sound Portd.7 , 300 , 800
'Sound Portd.7 , 300 , 200
Delay
Servopos = Center
Servo(1) = Servopos
Servo(2) = Center2
'Sound Portd.7 , 300 , 400
'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
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 A2_r > A2_l Then 'wenn links mehr platz als rechts dann zurückfahren und nach links drehen
'stop
Pwm1a = 0
Pwm1b = 0
Servo(2) = Unten
'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
End If
'####################################################
Loop
Lesezeichen