ping code

epicjr77

Hi I am trying to run sonar and 2 bumper switches on my robot at the same time, how ever it seems like the switches only work at a certain point in the program. Alos I am confused how to adjust the range of the ping

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

Ping PIN 14
PingServo PIN 0
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 1
PULSOUT 0, sweepcount
PAUSE 20
NEXT
GOSUB GetSonar
NEXT

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

IF (IN10 = 1) THEN ' whisker contacts

GOSUB Back_Up ' Back up & turn right
GOSUB Turn_Right
GOSUB Turn_Right


ELSEIF (IN9 = 1) THEN ' whisker contacts

GOSUB Back_Up ' Back up & turn right
GOSUB Turn_left
GOSUB Turn_Left

ENDIF



LOOP

'

---

Sub

---

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

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

RETURN

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

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

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

Back_Up:
FOR pulseCount = 0 TO 40
PULSOUT LeftServo, 650
PULSOUT RightServo, 850
PAUSE 20
NEXT

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

Franklin

Rawdist is the return from the ping and the faster it returns the closer the object is. You use this value to make your choices. As to the wiskers they are called very infrequently in your code, you need to move them to a spot where they are checked every time your program does something like moving forward or left or right.

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

epicjr77

how would you recommend moving it? Should I put the switch check in my sub routines?

Post Edited (epicjr77) : 7/9/2008 1:54:14 AM GMT