Forum Update - Announcement about May 10th, 2018 update and your password.

Need help combining 2 programs for boe-bot

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

  • 3 Comments sorted by Date Added Votes
  • ercoerco Posts: 19,158
    A fun and juicy problem! What have you tried so far?
    "When you make a thing, a thing that is new, it is so complicated making it that it is bound to be ugly. But those that make it after you, they don’t have to worry about making it. And they can make it pretty, and so everybody can like it when others make it after you."

    - Pablo Picasso
  • 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
  • Here is the file from above.
Sign In or Register to comment.