BACK
Source Code
'File: line_follow.bas
'Author: Kerwin Lumpkins
' This program drives an AVR BotBoard to take input from
' 3 IR emitter/phototransistor pairs and runs 2 servo motors
' to keep the robot following a black line on a white background
'declare the subroutines that do the real work
'check subroutines get the sensor values and store result to the Lftfnd
'Rtfnd and Cen variables. 1 = black, 0 = white
Declare Sub Checkrightsensor
Declare Sub Checkleftsensor
Declare Sub Checkcenter
'locomotion functions. sends out the values to the servo control IC with
'the printbin function. Forward is Right motor CW/Left motor CCW (remember
'that they are on opposite sides of the center axis of the robot so they
'spin in different directions to get the motors to turn in the same direction
'LEft turn is left motor CCW, Right motor CCW. etc
Declare Sub Rightturn
Declare Sub Leftturn
Declare Sub Forward
Declare Sub Backup
'Get_sensors sub gets values from all three sensors
Declare Sub Get_sensors
'decide sub arbitrates when in lost state
Declare Sub Decide
'declare the variables
Dim A As Byte, B1 As Byte, C(3) As Byte
Dim Rtfnd As Byte
Dim Lftfnd As Byte
Dim Cen As Byte
Dim State As Byte
Dim Laststate As Byte
Dim Cnt As Byte
'configuration stuff sets up the ports
Config Portd = &B1111110
'PD0 = RxD, Input
'PD1 = TxD, Output
'PD2 = output, don't need IRPD module
'PD3 = output, don't need IRPD module
'PD4 = output
'PD5,6 = outputs
' set pins B.0 and B.1 as inputs since they are comparator inputs
Config Pinb.0 = Input
Config Pinb.1 = Input
'first init the serial chip to disable servos 0 and 1
Open "comb.3:2400,8,n,1" For Output As #1
Config Spi = Soft , Din = Portb.6 , Dout = Portb.5 , Clock = Portb.7
Config Pinb.4 = Output
Spiinit
' main routine, gets state of sensors and decides what to do
Main:
Acsr = &B00010000
Rtfnd = 0
Lftfnd = 0
Cen = 0
State = 0
Laststate = 2
'Print "in main" + Chr(13)
'the do loop runs continuously
Do
Get_sensors
Select Case State
Case 0: Decide
Case 1: Rightturn
Case 2: Forward
Case 3: Rightturn
Case 4: Leftturn
Case 5: Backup
Case 6: Leftturn
Case 7: Backup
Case Else: Backup
End Select
'store the current state to the var laststate, used by decide routine
Laststate = State
Loop
End 'end program
'gets the sensors value and then puts them all into one byte variable
'called state. checkrightsensor routine stores 1 or 0 to Rtfnd variable
'The Lftfind var gets shifted left 2 times, center shifts once and then
'they are all added into one variable state
Sub Get_sensors()
Checkrightsensor
Checkleftsensor
Checkcenter
Shift Lftfnd, Left, 2
Shift Cen, Left, 1
State = Lftfnd + Cen
State = State + Rtfnd
End Sub
Sub Forward()
Printbin #1 , 0
Printbin #1 , 64
Printbin #1 , 63 'CCW on left
Printbin #1 , 66 'CW on right motor
Waitms 10
Printbin #1 , 0
Waitms 5
Return
End Sub
Sub Leftturn()
Printbin #1 , 0
Printbin #1 , 64
Printbin #1 , 1 'CW on left
Printbin #1 , 66 'CW on right
Waitms 10
Printbin #1 , 0
Waitms 20
Return
End Sub
Sub Rightturn()
Printbin #1 , 0
Printbin #1 , 64
Printbin #1 , 63 'CCW on left
Printbin #1 , 127 'CCW on right
Waitms 10
Printbin #1 , 0
Waitms 20
Return
End Sub
Sub Backup()
Printbin #1 , 0
Printbin #1 , 64
Printbin #1 , 1 'CW on left
Printbin #1 , 127 'CCW on right
Waitms 10
Printbin #1 , 0
Waitms 5
Return
End Sub
'do A/D conversion on left sensor, if above trigger value Lftfind = 1
(black)
Sub Checkleftsensor()
Lftfnd = 0
'Print "in check left" ; Chr(13)
'Tell 3202 to do a reading
A = &B00001100
For B1 = 1 To 3
Portb 0.4 = 0
Spiout A, 1
Spiin C(1), 2
Portb 0.4 = 1
C(3) = C(1)
Shift C(1), Right, 4
Shift C(2), Right, 4
Shift C(3), Left, 4
C(2) = C(2) Or C(3)
'Print C(1) ; " " ; C(2) ; Chr(13)
Incr B1
If C(1) >= 4 Then
Lftfnd = 1
Portd 0.6 = 1
B1 = 3
Else
Portd 0.6 = 0
End If
Waitms 10
Next B1
Return
End Sub
'do A/D conversion on right sensor, if above trigger value Lftfind =
1 (black)
Sub Checkrightsensor()
Rtfnd = 0
'Print "in check rt" ; Chr(13)
A = &B00001110
For B1 = 1 To 3
Portb 0.4 = 0
Spiout A, 1
Spiin C(1), 2
Portb 0.4 = 1
C(3) = C(1)
Shift C(1), Right, 4
Shift C(2), Right, 4
Shift C(3), Left, 4
C(2) = C(2) Or C(3)
'Print C(1) ; " " ; C(2) ; Chr(13)
'Print "c1 is " ; C(1) ; Chr(13)
If C(1) > 4 Then
Rtfnd = 1
Portd 0.4 = 1
B1 = 3
Else
Portd 0.4 = 0
End If
Waitms 10
Next B1
Return
End Sub
'get center sensor value from on board comparator of the 2313
Sub Checkcenter()
Cen = 0
Waitms 10
Acsr 0.4 = 1
If Acsr.5 = 1 Then
Cen = 1
Portd 0.5 = 1
Else
Portd 0.5 = 0
End If
'Print "cen is " ; Cen ; Chr(13)
Return
End Sub
'decide subroutine is called when no sensor has the line which is the
'lost state. This routine then looks at the laststate and makes a decision
'based on it
Sub Decide()
If Laststate = 1 Then
GoSub Rightturn
ElseIf Laststate = 2 Then
GoSub Backup
ElseIf Laststate = 3 Then
GoSub Rightturn
ElseIf Laststate = 4 Then
GoSub Leftturn
ElseIf Laststate = 6 Then
GoSub Leftturn
Else
GoSub Backup
End If
End Sub
BACK |
