Help with code!!

Sango

Can someone check this code? It is for a non-tactile robot that needs to go near but not touch the walls. I also need some help on which variables will I need to put to make it turn if it senses one side or another· Please help! THANK YOU!!


' {$STAMP BS2px}
' {$PBASIC 2.5}
·' =========================================================================
'
' File....... SRF04_Demo.BS2
' Purpose.... Devantech SRF04 Ultrasonic Range Finder
' Author..... Parallax, Inc. (Copyright 2003 - All Rights Reserved)
' E-mail..... support@parallax.com
' Started.... 06 MAR 2002
' Updated.... 01 OCT 2003
'
'
' =========================================================================
'

---

[noparse][[/noparse] Program Description ]

---

'
' This program uses the Devantech SRF04 to measure the distance between the
' unit and a target. Display is raw value, centimeters, and inches.
'

---

[noparse][[/noparse] Revision History ]

---

'
' 01 OCT 2003 : Updated for PBASIC 2.5 and for any BASIC Stamp module
'

---

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

---

'
Trigger PIN 0
Echo PIN 1
'Trigger2 PIN 2'
'Echo2 PIN 3'
'Trigger3 PIN 4'
'Echo3 PIN 5'
Trig10 CON 5
ToCm CON 78
'

---

[noparse][[/noparse] Constants ]

---

#SELECT $STAMP
#CASE BS2, BS2E
Trig10 CON 5 ' trigger pulse = 10 uS
ToCm CON 30 ' conversion factor to cm
#CASE BS2SX, BS2P
Trig10 CON 13
ToCm CON 78
#CASE BS2PE
Trig10 CON 5
ToCm CON 31
#ENDSELECT
'

---

[noparse][[/noparse] Variables ]

---

samples VAR Nib ' loop counter
pWidth VAR Word ' pulse width from sensor
rawDist VAR Word ' filtered measurment
cm VAR Word ' centimeters
inches VAR Word
pulscount VAR Word
'

---

[noparse][[/noparse] Initialization ]

---

Setup:
LOW Trigger
DEBUG CLS,
"Devantech SRF04 Demo", CR,
"

---

", CR,
"Raw........... ", CR,
"Centimeters... ", CR,
"Inches........ "
'

---

[noparse][[/noparse] Program Code ]

---

Main:
DO
GOSUB Get_Sonar ' take sonar reading
DEBUG CRSRXY, 15, 2, DEC rawDist, CLREOL
cm = rawDist / ToCm ' convert to centimeters
DEBUG CRSRXY, 15, 3, DEC cm, CLREOL
inches = cm */ $03EF ' x 3.937 (to 0.1 inches)
DEBUG CRSRXY, 15, 4,
DEC inches / 10, ".", DEC1 inches,
CLREOL
PAUSE 250 ' delay between readings
IF (rawDist > 3) THEN
GOSUB Forward
IF (rawDist ) THEN
GOSUB Backwards
ENDIF
ENDIF
LOOP
END
'

---

[noparse][[/noparse] Subroutines ]

---

Get_Sonar:
rawDist = 0 ' clear measurement
FOR samples = 1 TO 5 ' take five samples
PULSOUT Trigger, Trig10 ' 10 uS trigger pulse
#SELECT $STamp
#CASE BS2, BS2E
RCTIME Echo, 1, pWidth ' measure pulse
#CASE #ELSE
PULSIN Echo, 1, pWidth ' measure pulse
#ENDSELECT
rawDist = rawDist + (pWidth / 5) ' simple digital filter
PAUSE 10 ' minimum period between
NEXT
RETURN
Forward:
FOR pulscount = 1 TO 500
PULSOUT 13, 550
PULSOUT 12, 650
PAUSE 10
NEXT
RETURN
Backwards:
FOR pulscount = 1 TO 500
PULSOUT 12, 650
PULSOUT 13, 550
PAUSE 10
NEXT
RETURN
·

Franklin

If you ATTACH your code using the attachment manager it would make it much easier to test and follow. Please do.

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

Sango

Sorry. I am new. Here it is (if I did it right):

Franklin

Have you run this code yet? What did it do that you want to change? As far as I can see it looks OK.

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

Sango

I have run it. It is supposed to sense the walls and then move in accordance to the distance from the wall. The robot, however, is not sensing or moving.

SRLM

Try these:

Line 74, change to else (without condition)
Line 76, remove it
Lines 99 and 106 change to pause 20

Your sonar routine isn't correct. For one, you take five readings and only ever use the last one. Also, you have preprocessor commands in there that have not effect on the code executed. You'll want to fix that.

Are you sure that rawDistance is on such a small scale? You give it a word sized variable, but you test around the value 3. Something's wrong there.

Also, bad (or no) formatting is of the devil as far as programming is concerned. You'll want to format your code nicely (with tabs) and you'll catch some of these mistakes.

Post Edited (SRLM) : 3/29/2009 4:51:45 AM GMT