'{$STAMP BS2}
'{$PBASIC 2.5}

'This program is set to use only 5 of the LFM sensors
'They are placed at: 10100100101
'                    0         0
'The jumper set to 5

LMotor  CON 13         'Left servo pin
RMotor  CON 12         'Right servo pin

LEDon CON 0             'Constant to turn LED on
LEDoff  CON 1           'Constant to turn LED off

Wline CON 0             'Use to sense white lines on black
Bline CON 1             'Use to sense black lines on white
LFmode  CON Wline       'Set which line type

MStop          CON 750   'Basic Speed setting for servos, see the CBA manual
Speed100L      CON 870   'Replace these values with the ones you found in testing
Speed075L      CON 831
Speed050L      CON 815
Speed025L      CON 804
Speed010L      CON 793
Speed100R      CON 500
Speed075R      CON 650
Speed050R      CON 690
Speed025R      CON 705
Speed010R      CON 720

PulseWidthRt  VAR Word    'Variables for pulse widths to servos
PulseWidthLt  VAR Word

LEDPos     VAR Nib           'The rest of the variables
LFMBits     VAR Byte
lastBits  VAR Byte
steer       VAR Nib

Initialize:         'Initialize pins for LF module, P0 is input, P1 - P5 output
OUTL = %00111110
DIRL = %00111110
LastBits = %00100   'LastBits keeps track of where the line was last, initialize to center

PAUSE 2000          'Pause at start of program to allow time to set robot down


'*** This is the start of the main program loop ***
DO
   GOSUB Read_LFM   'Just as it sounds, read the LF module to see where line is

  IF LFMBits = 0 THEN       'Off the line is a special case. This changes the LF reading to think
         LFMBits = lastBits      'it was still on the line in the state it was just before it went off the line.
  ENDIF                   'That way it keep turning in the correct direction, while still looking for the line.

  steer = NCD LFMBits         'Looks to see which sensor the line is under
                                       'NCD takes the highest bit set in the variable lfBits and sets steer to its position.
                                        'as example %00001000 will set steer to 4

   'In the branch below we go to the pulse that is needed based on where the line is
   BRANCH Steer - 1, [Hard_left, left, Straight, Right, Hard_Right]      'Minus 1 because Branch starts at 0
   BackFromBranch:                 'Branch has to come back to a label

   PULSOUT Lmotor, PulseWidthLt    'Send pulse to left
   PULSOUT Rmotor, PulseWidthRt    'Send pulse to right
   lastBits = LFMBits                 'Save the LF reading so we can use it later if needed
LOOP


'---- Subs-----
Read_LFM:
LFMBits = 0                               'Clear last reading
FOR LedPos = 1 TO 5                      'Setup to loop through each sensor LED read only 5 sensors
   OUTL.LOWBIT(ledPos) = LEDon            'Turn LED on
   PAUSE 1                                'Pause so sensor can respond
   LFMBits.LOWBIT(ledPos) = IN0 ^ LFmode   'Read state of sensor, save into the bit position of that sensor
   OUTL = OUTL | %01111110                'Turn off any and all sensor LEDs
NEXT
LFMBits = LFMBits >>1                      'When done with reading shift bits right one because
RETURN                                   ' sensors start at P1 not P0

'* The 5 posabilities of steering are here*
Hard_Right:
  PulseWidthLt = Speed100L
  PulseWidthRt = Speed010R
  GOTO BackFromBranch

Right:
  PulseWidthLt = Speed100L
  PulseWidthRt = Speed025R
  GOTO BackFromBranch

Straight:
  PulseWidthLt = Speed100L
  PulseWidthRt = Speed100R
  GOTO BackFromBranch

Left:
  PulseWidthLt = Speed025L
  PulseWidthRt = Speed100R
  GOTO BackFromBranch

Hard_Left:
  PulseWidthLt = Speed010L
  PulseWidthRt = Speed100R
  GOTO BackFromBranch