Oto kod:
Kod: Zaznacz cały
'4x4 Line Follower
'Chibots line following Contest 2003
'Wright Hobbies, LLC Copyright 2003
'eddy@wrighthobbies.com
'Variables
'Line Detectors
Dim Lineflag As Byte
Dim I As Byte , X As Byte
Dim Adcvalue As Word , Channel As Byte , Progspeed As Byte
Dim Progsteer As Byte
Dim Lastadc(5) As Word
Dim Calhi(5) As Word , Callow(5) As Word , Threshold(5) As Word
Dim Temp As Word
Dim Lastlineflag As Byte
Dim Overshoot As Boolean
Dim Lostline As Word
Dim Flagleft As Boolean
Dim Flagright As Boolean
'Constants
Const Center = 95
Const Smallleft = 94
Const Mediumleft = 85
Const Largeleft = 75
Const Hardleft = 70
Const Smallright = 96
Const Mediumright = 107
Const Largeright = 117
Const Hardright = 122
'For Calibration
Const Fullright = 135
Const Fullleft = 60
Const Neutral = 100
Const Veryslow = 95
Const Slow = 92
Const Mediumslow = 91
Const Medium = 90
Const Mediumfast = 89
Const Fast = 88
Const Superfast = 80
Const Slowreverse = 110
Const Fastreverse = 150
'Declarations
Declare Sub Doadc()
Declare Sub Waitforbutton()
'Aliases
Redled Alias Portd.6
Greenled Alias Portd.5
Buttonleft Alias Pinb.1
Buttonright Alias Pinb.2
Config Adc = Single , Prescaler = Auto , Reference = Avcc
Config Servos = 2 , Servo1 = Portb.0 , Servo2 = Portd.7 , Reload = 10
Config Portb = Output
Config Portd = Output
Config Pinb.1 = Input
Config Pinb.2 = Input
Portb.1 = 1
Portb.2 = 1
Servo(1) = Center
Enable Interrupts
Start Timer1
Reset Greenled
Reset Redled
Progspeed = 0
X = 0
Do 'Wait for program the speed setting
Debounce Buttonleft , 0 , Program , Sub
Debounce Buttonright , 0 , Complete , Sub
Loop Until X = 1
Set Redled
Print " " ; "Speed selection: " ; Progspeed
Reset Redled
Reset Greenled
Waitms 100
Set Redled
Set Greenled
Waitms 100
Reset Redled
Reset Greenled
Waitms 100
Set Redled
Set Greenled
Waitforbutton
Progsteer = Progspeed
Progspeed = Progspeed * 2
Servo(1) = Center
Channel = 0
Start Adc
'Initial Read ignored
Adcvalue = Getadc(0)
'Calibrate sensors
Channel = 0
'Print "Starting Calibration"
For I = Fullleft To Fullright Step 2 'Begin scan for auto-calibration of sensors
Servo(1) = I
Waitms 50
For Channel = 0 To 4
Lastadc(channel + 1) = Getadc(channel)
Next Channel
For X = 1 To 5 'Scan through adc values and capture the highs and lows
If Calhi(x) = 0 Then
Calhi(x) = Lastadc(x)
Callow(x) = Lastadc(x)
End If
If Lastadc(x) > Calhi(x) Then Calhi(x) = Lastadc(x)
If Lastadc(x) < Callow(x) Then Callow(x) = Lastadc(x)
Next X
Next I
Servo(1) = Center
Channel = 0
For I = 1 To 5 'Determine the threshold for line/no lin
Threshold(i) = Calhi(i) - Callow(i)
Threshold(i) = Threshold(i) / 2
Threshold(i) = Calhi(i) - Threshold(i)
Threshold(i) = 700
Next I
Servo(1) = Center
Waitforbutton
'******************************
'Main loop
'******************************
Servo(2) = Neutral
'Shake for 2 seconds before going
For I = 1 To 200
Servo(1) = Center - 1
Waitms 5
Servo(1) = Center + 1
Waitms 5
Next I
Servo(2) = Fast
Do 'Main Program Loop
Doadc 'Check the sensors
If Lastadc(1) < Threshold(1) Then Set Lineflag.0 Else Reset Lineflag.0
If Lastadc(2) < Threshold(2) Then Set Lineflag.1 Else Reset Lineflag.1
If Lastadc(3) < Threshold(3) Then Set Lineflag.2 Else Reset Lineflag.2
If Lastadc(4) < Threshold(4) Then Set Lineflag.3 Else Reset Lineflag.3
If Lastadc(5) < Threshold(5) Then Set Lineflag.4 Else Reset Lineflag.4
'Print Bin(lineflag)
If Overshoot = 0 Then Lastlineflag = Lineflag 'Save results if we're not overshooting the corner
Select Case Lineflag
Case &B00000 'No line
'If the bot loses the line, make steering no changes
Servo(2) = Slow - Progspeed
If Lastlineflag < 4 Then
Overshoot = 1 'Overshot a righthand corner
Elseif Lastlineflag > 4 Then
Overshoot = 1 'Overshot a left hand corner
End If
Case &B00100 'Line under center sensor
Lostline = 0
Overshoot = 0
Servo(1) = Center
Servo(2) = Mediumfast - Progspeed
'Right Sensors
Case &B00110
Overshoot = 0
Servo(1) = Smallright
Servo(2) = Mediumfast - Progspeed
Case &B00010
Overshoot = 0 'Line under midright sensor
Servo(1) = Mediumright
Servo(2) = Mediumfast - Progspeed
Case &B00011 'Line between midright and right sensor
If Overshoot = 0 Then
Servo(1) = Largeright
Servo(2) = Medium - Progspeed
Else
Servo(1) = Mediumright
Servo(2) = Medium - Progspeed
End If
Case &B00001 'Line under right sensor
If Overshoot = 0 Then
Servo(1) = Hardright
Servo(2) = Mediumslow - Progspeed
Else
Servo(1) = Largeright
Servo(2) = Mediumslow - Progspeed
End If
'Left Sensors
Case &B01100 'Line between center and midleft sensor
Overshoot = 0
Servo(1) = Smallleft
Servo(2) = Mediumfast - Progspeed
Case &B01000 'Line under midleft sensor
Overshoot = 0
Servo(1) = Mediumleft
Servo(2) = Mediumfast - Progspeed
Case &B11000 'Line between midleft and left sensor
If Overshoot = 0 Then
Servo(1) = Largeleft
Servo(2) = Medium - Progspeed
Else
Servo(1) = Center
Servo(2) = Medium - Progspeed
End If
Case &B10000 'Line under left sensor
If Overshoot = 0 Then
Servo(1) = Hardleft
Servo(2) = Mediumslow - Progspeed
Else
Servo(1) = Largeleft
Servo(2) = Mediumslow - Progspeed
End If
Case &B11111 'This should never happen, probably an error. Turn in circle to find line
Servo(1) = Largeleft
Servo(2) = Slow - Progspeed
End Select
Loop
End
'****************************************
'Subroutines
'****************************************
Sub Doadc()
For X = 0 To 4 'Cycle through sensors
' Temp = 0
' For I = 0 To 2 'Take 3 samples
' Adcvalue = Getadc(x)
' Toggle Led1
' Temp = Temp + Adcvalue
' Next I
' Temp = Temp / 3
' Lastadc(x + 1) = Temp 'Average the 3 samples
' Adcvalue = Getadc(x)
Lastadc(x + 1) = Getadc(x)
Next X
End Sub
Sub Waitforbutton()
Cont1:
'Print "Checking Button"
Debounce Buttonleft , 0 , Cont2
Debounce Buttonright , 0 , Cont2
Goto Cont1
Cont2:
Print "Button Pushed"
End Sub
Complete:
X = 1
Return
Program:
Incr Progspeed
If Progspeed > 8 Then Progspeed = 0
For I = 0 To Progspeed
Reset Redled
Waitms 100
Set Redled
Waitms 100
Next I
ReturnMógłby ktoś powiedzieć gdzie tu zachodzi autokalibracja i jak się odbywa. Resztę sobie skonfiguruje (zastosuje zamiast serw PWM itd.)