Shop OBEX P1 Docs P2 Docs Learn Events
Need help combining 2 programs for boe-bot — Parallax Forums

Need help combining 2 programs for boe-bot

JC126792JC126792 Posts: 3
in Robotics
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

Comments

  • ercoerco Posts: 20,250
    A fun and juicy problem! What have you tried so far?
  • JC126792JC126792 Posts: 3
    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
  • JC126792JC126792 Posts: 3
    Here is the file from above.
    bs2
    bs2
    very close.bs2
    3K
Sign In or Register to comment.