QTI Navigation

sbao26975

I've got a bot setup with 4 QTI sensors so that it tracks a black line. My objective is to make the bot turn right the first two times when it reaches a T intersection (CASE %1111), keep going forward the next two times, turn right the next four times, keep going forward the two next times, right next four, etc.

I've looked into using a counter variable but I can't seem to get that to work. I understand that I have to implement something for CASE %1111 but I'm rather new to this...help would be appreciated!

Here is my current code:

' {$STAMP BS2sx}
' {$PBASIC 2.5}

' LineFollowWithCheckQtis.bs2
' Navigates based on values acquired with the Check_Qtis subroutine.




qtis VAR Nib ' qti black/white states
OUTB = %1111 ' Set OUTB bits to 1

' Main DO...LOOP
GO
GOSUB Check_Qtis ' Get QTI states

SELECT qtis ' Control servo speeds/directions
CASE %1000 ' Rotate right
PULSOUT 13, 600
PULSOUT 12, 600
CASE %1100 ' Pivot right
PULSOUT 13, 750
PULSOUT 12, 650
CASE %0100 ' Curve right
PULSOUT 13, 800
PULSOUT 12, 650
CASE %0110 ' Straight ahead
PULSOUT 13, 900
PULSOUT 12, 600
CASE %0010 ' Curve left
PULSOUT 13, 900
PULSOUT 12, 700
CASE %0011 ' Pivot left
PULSOUT 13, 900
PULSOUT 12, 750
CASE %0001 ' Rotate left
PULSOUT 13, 900
PULSOUT 12, 900
CASE %1111
PAUSE 100
PULSOUT 13, 900
PULSOUT 12, 600
CASE ELSE ' Do nothing
PAUSE 3
ENDSELECT
LOOP

Check_Qtis:
' Result -> qtis variable. 0 means white surface, 1 means black
' surface.

DIRB = %1111 ' P7..P4 -> output
PAUSE 0 ' Delay = 230 us
DIRB = %0000 ' P7..P4 -> input
PAUSE 0 ' Delay = 230 us
' PULSOUT UnusedPin, 0 ' Delays = 208 + (Duration*2) us
qtis = INB ' Store QTI outputs in INB

RETURN