PING sensor and servo motor

aznstormin

help!! how do i make them work together? everytime i have an if statement it's supposed to·move back if something is in front of it.....but it doesn't go to my navagation routine. any help to make it work?


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

DEBUG "Program Running!"

'

---

[noparse][[/noparse] Variables ]

---

' Declare variables for storing measured RC times of the
' left & right photoresistors.
timeLeft VAR Word
timeRight VAR Word
'average VAR·· Word
difference··· VAR· Word

rawDist········ VAR···· Word··················· ' raw measurement
inches········· VAR···· Word
cm············· VAR···· Word

counter······· VAR··· Word

'

---

[noparse][[/noparse] Initialization ]

---

FREQOUT 4, 2000, 3000


'

---

[noparse][[/noparse] I/O Definitions ]

---

Ping··········· PIN···· 15


'

---

[noparse][[/noparse] Constants ]

---

#SELECT $STAMP
· #CASE BS2, BS2E
··· Trigger···· CON···· 5······················ ' trigger pulse = 10 uS
··· Scale······ CON···· $200··················· ' raw x 2.00 = uS
· #CASE BS2SX, BS2P, BS2PX
··· Trigger···· CON···· 13
··· Scale······ CON···· $0CD··················· ' raw x 0.80 = uS
· #CASE BS2PE
··· Trigger···· CON···· 5
··· Scale······ CON···· $1E1··················· ' raw x 1.88 = uS
#ENDSELECT

RawToIn········ CON···· 889···················· ' 1 / 73.746 (with **)
RawToCm········ CON···· 2257··················· ' 1 / 29.034 (with **)

IsHigh········· CON···· 1······················ ' for PULSOUT
IsLow·········· CON···· 0

'

---

[noparse][[/noparse] Main Routine ]

---

DO
GOSUB Test_Photoresistors
GOSUB Average_And_Difference
GOSUB Get_Sonar···························· ' get sensor value
··· inches = rawDist ** RawToIn················ ' convert to inches
··· cm = rawDist ** RawToCm
··· DEBUG CRSRXY, 15, 3,······················· ' update report screen
········· DEC rawDist, CLREOL,
········· CRSRXY, 15, 4,
········· DEC inches, CLREOL,
········· CRSRXY, 15, 5,
········· DEC cm, CLREOL·················· ' convert to centimeters
·· ' PAUSE 100
·IF (rawDist > 450) THEN
·GOSUB Navigate
·ELSE
·FOR counter = 1 TO 120
·· PULSOUT 13, 650
·· PULSOUT 12, 850
·· PAUSE 20
·NEXT
·FOR counter = 1 TO 70
·· PULSOUT 13, 650
·· PULSOUT 12, 650
·· PAUSE 20
· NEXT

ENDIF
LOOP

'

---

[noparse][[/noparse] Subroutine - Test_Photoresistors ]

---

Test_Photoresistors:
HIGH 6 ' Left RC time measurement.
PAUSE 3
RCTIME 6,1,timeLeft
HIGH 3 ' Right RC time measurement.
PAUSE 3
RCTIME 3,1,timeRight
RETURN

'

---

[noparse][[/noparse] Subroutine - Navigate ]

---

Average_And_Difference:
difference = timeRight + timeLeft /2
difference = difference / 6
RETURN

'

---

[noparse][[/noparse] Subroutine - Navigate ]

---

Navigate:
'shadow sig. stronger on left detector, turn right
IF (timeLeft > timeRight + difference) THEN
PULSOUT 13, 850
PULSOUT 12, 850
'shadow sig. stronger on right detector, turn left
ELSEIF (timeRight > timeLeft + difference) THEN
PULSOUT 13, 650
PULSOUT 12, 650

'when it hits light
'ELSEIF (timeRight < 300) AND (timeLeft < 300) THEN
'PULSOUT 13, 750
'PULSOUT 12, 750
'FREQOUT 4, 2000, 3000
'FREQOUT 4, 2000, 3000
'shadows in same neighborhood of intensity on both detectors
ELSE
PULSOUT 13, 850
PULSOUT 12, 650
ENDIF
PAUSE 10

RETURN

'

---

[noparse][[/noparse] Subroutines ]

---

' This subroutine triggers the Ping sonar sensor and measures
' the echo pulse.· The raw value from the sensor is converted to
' microseconds based on the Stamp module in use.· This value is
' divided by two to remove the return trip -- the result value is
' the distance from the sensor to the target in microseconds.

Get_Sonar:
· Ping = IsLow································· ' make trigger 0-1-0
· PULSOUT Ping, Trigger························ ' activate sensor
· PULSIN· Ping, IsHigh, rawDist················ ' measure echo pulse
· rawDist = rawDist */ Scale··················· ' convert to uS
· rawDist = rawDist / 2························ ' remove return trip
· RETURN

·

Bruce Bates

aznstormin -

You've asked for help, but left us without much functional information fromthe execution of your program. What were the results of the DEBUG output when you ran the program?

It's entirely possible that you're receiving a fairly immediate bounce off the floor, or off of your undescribed platform which is less than your requirement of 450 mS in your IF statement. ONLY if it's GREATER than 450 ms away will you enter your navigation routine.

The other thing I can't seem to find in the Ping documentation is, what will it return if there is NO ECHO response at all, as might be seen in a completely open field environment. I'd LIKE to think it would return ZERO, but I can't find that so stated anywhere.

Regards,

Bruce Bates

Chris Savage

We should have a PING Sensor Demo using the BOE-Bot up by tomorrow.· I will post back here when it's ready, and maybe you can use it to figure out what you need.· It's pretty much done, just needs to be cleaned up a little.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Chris Savage
Parallax Tech Support
csavage@parallax.com

aznstormin

my apologies for not giving enough info n all of your help is appreciated, but i was in a rush, and actually got what i needed to be done. thank you tho.