Need help combining 2 programs for boe-bot

JC126792

hello, I have a qti and ping programs that i want to put together. How would i go about doing it. thanks.

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

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


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

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

SELECT qtis ' Control servo speeds/directions
CASE %10 ' Rotate right
PULSOUT 13, 750
PULSOUT 12, 850
PAUSE 20
CASE %00 ' Straight ahead
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
CASE %01 ' Curve left
PULSOUT 13, 650
PULSOUT 12, 750
PAUSE 20
CASE ELSE ' Do nothing
PAUSE 0
ENDSELECT
LOOP

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

DIRB = %11 ' P5 and P4 -> output
PAUSE 0 ' Delay = 230 us
DIRB = %00 ' P5 and P4 -> input
PAUSE 0 ' Delay = 230 us
' PULSOUT UnusedPin, 0 ' Delays = 208 + (Duration*2) us
qtis = INB ' Store QTI outputs in INB

RETURN






***************************************************************************************************************************

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

Ping PIN 15
PingServo PIN 14
LeftServo PIN 13
RightServo PIN 12

rawDist VAR Word
sweepcount VAR Word
x VAR Byte
pulseCount VAR Byte


DO
FOR sweepcount = 350 TO 1150 STEP 200
FOR x = 0 TO 5
PULSOUT 14, sweepcount
PAUSE 20
NEXT
GOSUB GetSonar
NEXT

FOR sweepcount = 950 TO 550 STEP 200
FOR x = 0 TO 5
PULSOUT 14, sweepcount
PAUSE 20
NEXT
GOSUB GetSonar
NEXT
LOOP

GetSonar:
LOW Ping
PULSOUT Ping, 5
PULSIN Ping, 1, rawDist

IF (rawDist < 600) THEN
IF (sweepcount < 750) THEN
GOSUB Turn_Left
ELSEIF (sweepcount > 750) THEN
GOSUB Turn_Right
ELSE
GOSUB Back_Up
ENDIF
ELSE
GOSUB Forward_Pulse
ENDIF

RETURN

Forward_Pulse:
FOR pulsecount = 0 TO 5
PULSOUT LeftServo, 850
PULSOUT RightServo, 650
PAUSE 5
NEXT
RETURN

Turn_Left:
FOR pulseCount = 0 TO 10
PULSOUT LeftServo, 650
PULSOUT RightServo, 750
PAUSE 5
NEXT
RETURN

Turn_Right:
FOR pulseCount = 0 TO 10
PULSOUT LeftServo, 750
PULSOUT RightServo, 850
PAUSE 5
NEXT
RETURN

Back_Up:
FOR pulseCount = 0 TO 3
PULSOUT LeftServo, 750
PULSOUT RightServo, 750
PAUSE 5
NEXT

FOR pulseCount = 0 TO 7
PULSOUT LeftServo, 650
PULSOUT RightServo, 850
PAUSE 5
NEXT
RETURN

erco

A fun and juicy problem! What have you tried so far?

JC126792

I made Roam with ping a subroutine so when the qti's read white surface the ping program starts. But it takes about 2 seconds unti the either qtis see black. i think its a pause issue.

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

Ping PIN 15
PingServo PIN 14
LeftServo PIN 13
RightServo PIN 12

rawDist VAR Word
sweepcount VAR Word
x VAR Byte
pulseCount VAR Byte

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

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

SELECT qtis
CASE %10 ' left QTI avoid black line turn right
PULSOUT 13, 750
PULSOUT 12, 850
PAUSE 20
CASE %00 ' surface is white Roam with Ping
IF qtis = 0 THEN Roam_Ping
CASE %01 ' right QTI avoid black line turn right
PULSOUT 13, 650
PULSOUT 12, 750
PAUSE 20
CASE %11 'Stop at black line
PULSOUT 13, 750
PULSOUT 12, 750
PAUSE 20
ENDSELECT
LOOP

Roam_Ping:
FOR sweepcount = 350 TO 1150 STEP 200
FOR x = 0 TO 5
PULSOUT 14, sweepcount
PAUSE 5
NEXT
GOSUB GetSonar
NEXT

FOR sweepcount = 950 TO 550 STEP 200
FOR x = 0 TO 5
PULSOUT 14, sweepcount
PAUSE 5
NEXT
GOSUB GetSonar
NEXT
RETURN

GetSonar:
LOW Ping
PULSOUT Ping, 5
PULSIN Ping, 1, rawDist

IF (rawDist < 350) THEN
IF (sweepcount < 750) THEN
GOSUB Turn_Left
ELSEIF (sweepcount > 750) THEN
GOSUB Turn_Right
ELSE
GOSUB Back_Up
ENDIF
ELSE
GOSUB Forward_Pulse
ENDIF
RETURN

Forward_Pulse:
FOR pulsecount = 0 TO 5
PULSOUT LeftServo, 785
PULSOUT RightServo, 715
PAUSE 20
NEXT
RETURN

Turn_Left:
FOR pulseCount = 0 TO 10
PULSOUT LeftServo, 650
PULSOUT RightServo, 750
PAUSE 10
NEXT
RETURN

Turn_Right:
FOR pulseCount = 0 TO 10
PULSOUT LeftServo, 750
PULSOUT RightServo, 850
PAUSE 10
NEXT
RETURN

Back_Up:
FOR pulseCount = 0 TO 3
PULSOUT LeftServo, 750
PULSOUT RightServo, 750
PAUSE 10
NEXT

FOR pulseCount = 0 TO 7
PULSOUT LeftServo, 650
PULSOUT RightServo, 850
PAUSE 10
NEXT
RETURN

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

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

RETURN

JC126792

Here is the file from above.