'--------------------------------------------------------------
'  file: folline.BAS
'  DLC 4/13/2003
'  Class TTT MicroAVR line follower demo code
'--------------------------------------------------------------

Dim A As Byte

Config Portb = &B10101111
Config Portd = &B11111000
'Port B pins
'B7 = SCK output
'B6 = MISO input
'B5 = MOSI output
'B4 = IRM input
'B3 = 40KHz IR modulation output
'B2 = Left IR LED enable output
'B1 = Right IR LED enable output
'B0 = LED signal output

'Port D pins
'D7 = N/A set to output
'D6 = Left servo output
'D5 = Right servo output
'D4 = LED signal output
'D3 = unassigned output
'D2 = Left line sensor input
'D1 = Center line sensor input
'D0 = Right line sensor input

$crystal = 10000000

'Sets 40KHz OC for IRPD LEDs
Config Timer1 = Timer , Prescale = 1 , Compare A = Toggle
Set Tccr1b.3
Compare1a = 125

'Getlines() looks at the three front sensors and determines where a line is
'under the robot. If the robot has move to the right of the line it returns
'a number -8, -16, -24 depending how far out it moved, if the robot has moved
'to the left then +8, 16 and 24 can be returned.  If the robot is directly
'over the line (center sensor) then 0 is returned.
Declare Sub Getlines
Dim Lines As Byte
Dim Llines As Integer
Dim Rlines As Integer
Dim Ldir As Byte
Dim Dir As Byte

'Setmotors() takes a URCP encoded velocity from -32 to +32 for each motor
'and sets the proper speed.  (-) is reverse, (+) is forward.
Declare Sub Setmotors(byval Lmot As Integer , Byval Rmot As Integer)
Dim Tmpa As Integer
Dim Rmotor As Integer
Dim Lmotor As Integer

Dim Tmotorl As Integer
Dim Tmotorr As Integer

'14.95us per step issued every 17ms
'smaller makes right go forward
'left motor center = 97, right = 97
Config Servos = 2 , Servo1 = Portd.6 , Servo2 = Portd.5 , Reload = 10
'To reduce the response to changes, set these numbers higher, to quicken the
'responses, set them lower.  10 seems to be a good compromise.
Rmotor = 10
Lmotor = 10
'Setmotors() uses URCP directions (-32 to + 32) to set motor direction and
'speed.  Negative number is backwards, positive is forwards.  0 = stop.
'Really only +/- 12 matter on a hobby servo.
Call Setmotors(lmotor , Rmotor)
'Set servo initial port values before starting interrupts.
Portd.5 = 0
Portd.6 = 0
Portb.0 = 0
'wait 5 seconds after power is applied
Waitms 5000
'start the servos now.
Enable Interrupts

Do
    Getlines

    If Rlines <> 0 Then
        'add the correction vector to the left motor and subract it from
        'the right motor.
        Tmotorl = Lmotor + Rlines
        Tmotorr = Rmotor - Rlines
    Else
        'Full speed ahead!
        Tmotorl = Lmotor + 10
        Tmotorr = Rmotor + 10
    End If

    Call Setmotors(tmotorl , Tmotorr)

Loop

End

Sub Getlines()
    'Reads in the line sensor values
    Lines = Pind And &H07
    If Lines = 2 Then
        Rlines = 0
        Dir = 0                                             'make no changes
        Return
    Elseif Lines > 3 Then
        Lines = Lines - 3                                   'we've moved to the left
        Dir = 1
    Elseif Lines > 0 Then
        Dir = 2                                             'we've moved to the right
    Else
        Dir = Ldir                                          'off the edge, assume last direction
    End If

    Ldir = Dir                                              'remember this direction
    Lines = 4 - Lines                                       'make larger the outside
    If Lines > 1 Then Lines = Lines - 1
    If Dir = 2 Then
        Rlines = -3 * Lines
        If Lines = 2 Then Rlines = -9
    Elseif Dir = 1 Then
        Rlines = 3 * Lines
        If Lines = 2 Then Rlines = 9
    Else
        Rlines = 0
    End If


End Sub Getlines

Sub Setmotors(lmot , Rmot)
    'uses URCP values -32 to +32 for motor speeds
    Dim Tlmotor As Byte
    Dim Trmotor As Byte

    Rmot = 0 - Rmot                                         'invert this one

    'Lmot = Lmot / 2
    'Tmpa = Lmot / 2
    'Lmot = Lmot + Tmpa
    Tlmotor = 98 + Lmot                                     'lmotor trimmed up one

    'Rmot = Rmot / 2
    'Tmpa = Rmot / 2
    'Rmot = Rmot + Tmpa
    Trmotor = 97 + Rmot
    Servo(1) = Tlmotor
    Servo(2) = Trmotor

End Sub Setmotors



                                                     'end program