Movement combine with wisker

1a2s3d4f

this is the movement we have.
' Robotics with the Boe-Bot - ShadowGuidedBoeBot.bs2
' Boe-Bot detects shadows cast by your hand and tries to follow them.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
FREQOUT 4, 2000, 3000

counter VAR Word
pulseCount VAR Word
' Start/restart signal.
'DO
'IF (IN6 = 0) AND (IN3 = 0) THEN ' Both detect shadows, forward.
Main:

'FOR counter = 1 TO 3
GOSUB Forward
GOSUB Right_Turn

GOSUB Forward1
GOSUB Right_Turn1

GOSUB Forward2
GOSUB Right_Turn2

GOSUB Forward3
GOSUB Right_Turn3

GOSUB Forward4
GOSUB Right_Turn4

GOSUB Forward5
GOSUB Right_Turn5

GOSUB Forward6
GOSUB Right_Turn6

GOSUB Forward7
GOSUB Right_Turn7


'PULSOUT 13, 850
'PULSOUT 12, 650
'ELSEIF (IN6 = 0) THEN ' Left detects shadow,
'PULSOUT 13, 760 ' pivot left.
'PULSOUT 12, 650
'ELSEIF (IN3 = 0) THEN ' Right detects shadow,
' PULSOUT 13, 650 ' pivot right.
'PULSOUT 12, 760
'ELSE
' PULSOUT 13, 750
'PULSOUT 12, 750
'ENDIF


'PAUSE 20 ' Pause between pulses.

'LOOP
'NEXT
END

Forward:
FOR pulseCount = 1 TO 64
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 10
RETURN


Right_Turn:
FOR pulseCount = 1 TO 45
PULSOUT 13, 760
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

Forward1:
FOR pulseCount = 1 TO 68
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 20
RETURN

Right_Turn1:
FOR pulseCount = 1 TO 46
PULSOUT 13, 760
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

Forward2:
FOR pulseCount = 1 TO 270
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 20
RETURN

Right_Turn2:
FOR pulseCount = 1 TO 115
PULSOUT 13, 830
PULSOUT 12, 700
NEXT
PAUSE 20
RETURN

Forward3:
FOR pulseCount = 1 TO 350
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 20
RETURN

Right_Turn3:
FOR pulseCount = 1 TO 46
PULSOUT 13, 760
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

'**********************************************************
Forward4:
FOR pulseCount = 1 TO 300
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 10
RETURN


Right_Turn4:
FOR pulseCount = 1 TO 47
PULSOUT 13, 760
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

Forward5:
FOR pulseCount = 1 TO 280
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 20
RETURN

Right_Turn5:
FOR pulseCount = 1 TO 45
PULSOUT 13, 760
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

Forward6:
FOR pulseCount = 1 TO 450
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 20
RETURN

Right_Turn6:
FOR pulseCount = 1 TO 116
PULSOUT 13, 830
PULSOUT 12, 700
NEXT
PAUSE 20
RETURN

Forward7:
FOR pulseCount = 1 TO 450
PULSOUT 13, 704
PULSOUT 12, 650
NEXT
PAUSE 20
RETURN

Right_Turn7:
FOR pulseCount = 1 TO 40
PULSOUT 13, 760
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

and this is the code for wisker:
'

---

[noparse][[/noparse] Title ]

---

' Robotics with the Boe-Bot - EscapingCorners.bs2
' Boe-Bot navigates out of corners by detecting alternating whisker presses.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
'

---

[noparse][[/noparse] Variables ]

---

pulseCount VAR Byte ' FOR...NEXT loop counter.
counter VAR Nib ' Counts alternate contacts.
old7 VAR Bit ' Stores previous IN7.
old5 VAR Bit ' Stores previous IN5.
'

---

[noparse][[/noparse] Initialization ]

---

FREQOUT 4, 2000, 3000 ' Signal program start/reset.
counter = 1 ' Start alternate corner count.
old7 = 0 ' Make up old values.
old5 = 1
'

---

[noparse][[/noparse] Main Routine ]

---

DO
' --- Detect Consecutive Alternate Corners

---

' See the "How EscapingCorners.bs2 Works" section that follows this program.
IF (IN7 <> IN5) THEN ' One or other is pressed.
IF (old7 <> IN7) AND (old5 <> IN5) THEN ' Different from previous.
counter = counter + 1 ' Alternate whisker count + 1.
old7 = IN7 ' Record this whisker press
old5 = IN5 ' for next comparison.
IF (counter > 4) THEN ' If alternate whisker count = 4,
counter = 1 ' reset whisker counter
GOSUB Back_Up ' and execute a U-turn.
GOSUB Turn_Left
GOSUB Turn_Left
ENDIF ' ENDIF counter > 4.
ELSE ' ELSE (old7=IN7) or (old5=IN5),
counter = 1 ' not alternate, reset counter.
ENDIF ' ENDIF (old7<>IN7) and
' (old5<>IN5).
ENDIF ' ENDIF (IN7<>IN5).
' --- Same navigation routine from RoamingWithWhiskers.bs2

---

IF (IN5 = 0) AND (IN7 = 0) THEN ' Both whiskers detect obstacle
GOSUB Back_Up ' Back up & U-turn (left twice)
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN ' Left whisker contacts
GOSUB Back_Up ' Back up & turn right
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN ' Right whisker contacts
GOSUB Back_Up ' Back up & turn left
GOSUB Turn_Left
ELSE ' Both whiskers 1, no contacts
GOSUB Forward_Pulse ' Apply a forward pulse
ENDIF ' and check again
LOOP
'

---

[noparse][[/noparse] Subroutines ]

---

Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,704
PULSOUT 12,650
PAUSE 20
RETURN

Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 46
PULSOUT 13, 830
PULSOUT 12, 700
PAUSE 20
NEXT
RETURN

Turn_Right:
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN

Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 704
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN

what we want to do is to combine this two code together. the result of this is that the robot is moving and cover the assign area. however, when it see a obstacle, it will move away and then continue moving to cover the assign area.

can you guy help me out. i try to figure it out but when i combine the code and burn it into the board, the robot does not behavior the way i wanted.

thank you.

Franklin

Just how does it behave and what is the actual code you are using? Please post as an attachment rather than inline. (it helps)

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
- Stephen