Line following robot code

[Anobium]

My code from my GCB line following robot. The robot has 1) three line sensors, 2) independent motor PWM and 3) speed control via a pot.

Works really well. I will post a YouTube video soon.

:-)

Anobium

' Working line robot with three optical sensors
' uses QRD1114 and a 10k POT for speed control.
'

#chip 16F690, 8
#config osc = INTRC_OSC_NOCLKOUT, MCLRE_OFF

'defines

#define Rmotorpmw PORTB.5
#define Lmotorpmw PORTB.6

#define pLftFDir PORTC.3
#define pLftRDir PORTC.4
#define pRgtFDir PORTC.5
#define pRgtRDir PORTC.6

#define AdcPortIn PORTC.7

#define LSensor Portc.0
#define RSensor Portc.1

'directions

dir PORTB.5 out
dir PORTB.6 out

dir PORTC.3 out
dir PORTC.4 out
dir PORTC.5 out
dir PORTC.6 out

dir PORTC.7 In

dir portc.0 In
dir portc.1 In

'variables

Dim oldstat As Word
Dim currentstat As Word
Dim temp As Word

Dim oldstat As Byte
Dim laststat As Byte

Dim lMotorSpeed As Byte
Dim rMotorSpeed As Byte
Dim lMove as Byte

' constants
#define lFast = 100
#define lSlow = 50
#define lStop = 0

'Serial settings
#define SerInPort PORTa.4
#define SerOutPort PORTa.5
'Set pin directions
Dir SerOutPort Out
Dir SerInPort In
'Set up serial connection
' emulator needs
' #define SendAHigh Set SerOutPort Off
' #define SendALow Set SerOutPort On
' #define RecAHigh SerInPort Off
' #define RecALow SerInPort On

#define SendAHigh Set SerOutPort Off
#define SendALow Set SerOutPort On
#define RecAHigh SerInPort Off
#define RecALow SerInPort On

init:
' supports emulator - needs to be INVERT
InitSer 1, r4800, 1+WaitForStart , 8, 1, none, invert
wait 5 ms
SerPrint 1, "Start:"
SerSend 1, 13
SerSend 1, 10

' flash the motors and leds
for ccount = 1 to 2
Set Lmotorpmw on
Set Rmotorpmw on
Wait 250 ms
Set Lmotorpmw off
Set Rmotorpmw off
Wait 250 ms
next

'Call the initialisation routine
InitMotorControl

'Main routine
' set moving forward
Gosub Forward
oldstat = 0
laststat = 0

'Main routine
main:
Do
' determine motor speed via ADC
lMotorSpeed = readad (9) / 25 * 10
if lMotorSpeed < 25 then
lMotorSpeed = 25
end if
rMotorSpeed = lMotorSpeed

' read current status of the three optical sensors
currentstat  = portc and 7
laststat = currentstat

' so, currentstat should have the direction of the robot

  select case currentstat
  case 0
      Gosub Forward
  case 1
      Gosub Forward

  case 2
      Gosub Forward
  case 3
        Gosub TurnLeft
      lMove = 1
  case 4
       Gosub Forward
  case 5
        Gosub Forward
  case 6
      Gosub TurnRight
      lMove = 2
  case 7
       ' lost the line - recover
       Select case lMove
              case 1
                   Gosub TurnLeft
              Case 2
                   Gosub TurnRight
              Case Else
                   Gosub MotorsStop
       end Select

  end select

Loop

'Setup PWM routine
Sub InitMotorControl
'Clear variables
lMotorSpeed = 0
rMotorSpeed = 0
PWMCounter = 0

'Add a handler for the interrupt
On Interrupt Timer0Overflow Call PWMHandler
'Set up the timer
InitTimer0 Osc, PS0_1/2
'Timer 0 starts automatically on a PIC

End Sub

'PWM sub
'This will be called when Timer 0 overflows
Sub PWMHandler

If rMotorSpeed > PWMCounter Then
    Set rmotorpmw On
Else
    Set rmotorpmw Off
End If

If lMotorSpeed > PWMCounter Then
    Set lmotorpmw On
Else
    Set lmotorpmw Off
End If

PWMCounter += 1
If PWMCounter = 100 Then PWMCounter = 0

End Sub

' Movements subs
SUB Forward

temp = PORTC and 120
if  temp <> 40 then
    SET pLftFDir ON
    SET pLftRDir OFF
    SET pRgtFDir oN
    sET pRgtRDir OFF

   #ifdef SerialOutput
    intoff
    SerSend 1,62
    SerSend 1,9
    wait 25 ms
    SerSend 1,"F"
    SerSend 1,9
    wait 25 ms
    SerPrint 1, temp
    SerSend 1,13
    SerSend 1,10
    IntOn
   #endif
end if

END SUB

SUB Reverse
SET pLftFDir OFF
wait 5 ms
SET pLftRDir ON
wait 5 ms
SET pRgtFDir oFF
wait 5 ms
sET pRgtRDir ON
wait 5 ms
END SUB

SUB SlewLeft
SET pLftFDir OFF
SET pLftRDir ON

SET pRgtFDir oN
sET pRgtRDir OFF

END SUB

SUB SlewRight
SET pLftFDir ON
SET pLftRDir OFF

SET pRgtFDir oFF
sET pRgtRDir ON

END SUB

SUB TurnLeft

temp = PORTC and 120
if  temp <> 32 then
    SET pLftFDir OFF
    SET pLftRDir OFF
    SET pRgtFDir oN
    sET pRgtRDir OFF

   #ifdef SerialOutput
    intoff
    SerSend 1,62
    SerSend 1,9
    wait 25 ms
    SerSend 1,"L"
    SerSend 1,9
    wait 25 ms
    SerPrint 1, temp
    SerSend 1,13
    SerSend 1,10
    IntOn
   #endif
end if

END SUB

SUB TurnRight
temp = PORTC and 120
if temp <> 8 then
SET pLftFDir ON
SET pLftRDir OFF
SET pRgtFDir oFF
sET pRgtRDir OFF

   #ifdef SerialOutput
    intoff
    SerSend 1,62
    SerSend 1,9
    wait 25 ms
    SerSend 1,"R"
    SerSend 1,9
    wait 25 ms
    SerPrint 1, temp
    SerSend 1,13
    SerSend 1,10
    IntOn
   #endif
end if

END SUB

SUB MotorsStop
SET pLftFDir OFF
SET pLftRDir OFF

SET pRgtFDir oFF
sET pRgtRDir OFF

END SUB