SRF04 sonar code problems

MarcVW

I'm new to programing and the basic stamp. I'm still at a cut paste and modify stage in learning to program. My SRF04 works fine but the pause comands in my code cause the boe-bot to stop and start quickly. Right now my goal is to just have my to IR senors guide the boe-bot which works fine. Once adding the code for the SRF04 to report to the computer it starts the start and stop. Any sugestions on how to read from the sonar at the same time as running the IR code.

' IR object detection ultrasonic assisted navigation
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
· Trigger···· CON 8
· Echo······· CON 9
· MoveTo····· CON 2
'

---

' Variables
'

---

· pulseLeft·· VAR Word
· pulseRight· VAR Word
· pWidth····· VAR Word··················· ' pulse width from sensor
· rawDist···· VAR Word··················· ' filtered measurment
· distance··· VAR Word··················· ' converted value
· temp······· VAR Word··················· ' value for RJ_print
· blips······ VAR Nib···················· ' loop counter for measurement
· digits····· VAR Nib

FREQOUT 4, 2000, 3000 ' Signal program start/reset.
'

---

' Initialization
'

---

Init:
· PAUSE 250
· DEBUG CLS
· DEBUG "Devantech SRF04 Demo", CR
· DEBUG "

---

", CR, CR
· DEBUG "Raw........... ", CR
· DEBUG "Inches........········ ", 34, CR
· DEBUG "Centimeters...········ cm", CR
'

---

' Main Program
'

---

Main:
DO
· GOSUB Get_Sonar······························ ' take sonar reading
··· DEBUG MoveTo, 15, 3
··· temp = rawDist
··· GOSUB RJ_Print······························· ' display raw value
· DEBUG MoveTo, 15, 4
··· distance = rawDist ** 8886··················· ' divide by 7.3746
··· temp = distance / 10
··· GOSUB RJ_Print······························· ' display inches
··· DEBUG ".", DEC1 distance
· DEBUG MoveTo, 15, 5
··· distance = rawDist ** 22572·················· ' divide by 2.9033
··· temp = distance / 10
··· GOSUB RJ_Print······························· ' display centimeters
··· DEBUG ".", DEC1 distance
· PAUSE 200···································· ' delay between readings
IF (IN5 = 1) AND (IN7 = 1) THEN····· ' Decide how to navigate.
· GOSUB Backup
· ELSEIF (IN5 = 1) THEN
··· GOSUB Left_Turn
· ELSEIF (IN7 = 1) THEN
··· GOSUB Right_Turn
· ELSE
··· GOSUB Forward
· ENDIF
··· PULSOUT 13,pulseLeft ' Apply the pulse.
··· PULSOUT 12,pulseRight
··· PAUSE 15
· LOOP ' Repeat main routine
'

---

' Subroutines
'

---

Get_Sonar:
··· rawDist = 0·································· ' clear measurement
··· FOR blips = 1 TO 5··························· ' take five samples
····· PULSOUT Trigger, 10························ ' 10 uS trigger pulse
····· PULSIN Echo, 1, pWidth····················· ' measure distance to target
····· rawDist = rawDist + (pWidth / 5)··········· ' simple digital filter
····· PAUSE 10··································· ' minimum period between pulses
··· NEXT
··· RETURN
Backup:·········································· 'Backup
····· HIGH 10
····· HIGH 1
····· pulseLeft = 650
····· pulseRight = 850
··· RETURN
Left_Turn:······································· ' Turn Left
····· HIGH 10
····· pulseLeft = 850
····· pulseRight = 850
··· RETURN
Right_Turn:······································ ' Turn Right
····· HIGH 1
····· pulseLeft = 650
····· pulseRight = 650
··· RETURN
Forward:········································· ' Forward
····· LOW 1
····· LOW 10
····· pulseLeft = 850
····· pulseRight = 650
··· RETURN
RJ_Print:········································ ' right justify
····· digits = 5
····· LOOKDOWN temp, <[noparse][[/noparse]0,10,100,1000,65535], digits
····· DEBUG REP " "\(5 - digits), DEC temp
··· RETURN

·

Jon Williams

That becomes a bit tricky -- you're dealing with lots of timing issues. The problem you're having is that the servos want to be updated every 20 milliseconds but you're not doing that, and every character you send to the Debug port costs you a millisecond. You've also added more PAUSE commands, one is up to 200 milliseconds.

Since you're new I'll suggest you start by updating the servos before and after you read the sonar. Remove any unnecessary PAUSE instructions that are just part of the SRF04 demo. And then subtract a millisecond for every character you send via DEBUG from the PAUSE 20 instructions you would normally have between servo updates.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jon Williams
Applications Engineer, Parallax

MarcVW

Thank you for the help Jon it is still a little jittery as it moves but has inproved. My goal is to have it working as well as the Fast IR roaming program from the Boe-Bot book. This is what I have now for code :
' IR object detection ultrasonic assisted navigation
' {$STAMP BS2}
' {$PBASIC 2.5}
DEBUG "Program Running!"
· Trigger···· CON 0
· Echo······· CON 1
· MoveTo····· CON 2
'

---

' Variables
'

---

· pulseLeft·· VAR Word
· pulseRight· VAR Word
· pWidth····· VAR Word··················· ' pulse width from sensor
· rawDist···· VAR Word··················· ' filtered measurment
· distance··· VAR Word··················· ' converted value
· temp······· VAR Word··················· ' value for RJ_print
· blips······ VAR Nib···················· ' loop counter for measurement
· digits····· VAR Nib

FREQOUT 4, 2000, 3000 ' Signal program start/reset.
'

---

' Initialization
'

---

Init:
· DEBUG CLS
· DEBUG "Raw........... ", CR
· DEBUG "Inches........········ ", 34, CR
'

---

' Main Program
'

---

Main:
DO
··· PULSOUT 13, pulseLeft ' Apply the pulse.
··· PULSOUT 12, pulseRight
··· PAUSE 10
· GOSUB Get_Sonar······························ ' take sonar reading
··· DEBUG MoveTo, 15, 3
··· temp = rawDist
··· GOSUB RJ_Print······························· ' display raw value
· DEBUG MoveTo, 15, 4
··· distance = rawDist ** 8886··················· ' divide by 7.3746
··· temp = distance / 10
··· GOSUB RJ_Print······························· ' display inches
··· DEBUG ".", DEC1 distance
IF (IN5 = 1) AND (IN7 = 1) THEN····· ' Decide how to navigate.
· GOSUB Backup
· ELSEIF (IN5 = 1) THEN
··· GOSUB Left_Turn
· ELSEIF (IN7 = 1) THEN
··· GOSUB Right_Turn
· ELSE
··· GOSUB Forward
· ENDIF
····· PULSOUT 13, pulseLeft ' Apply the pulse.
····· PULSOUT 12, pulseRight
··· PAUSE 10
· LOOP: ' Repeat main routine
'

---

' Subroutines
'

---

Get_Sonar:
··· rawDist = 0·································· ' clear measurement
··· FOR blips = 1 TO 5··························· ' take five samples
····· PULSOUT Trigger, 10························ ' 10 uS trigger pulse
····· PULSIN Echo, 1, pWidth····················· ' measure distance to target
····· rawDist = rawDist + (pWidth / 5)··········· ' simple digital filter
····· PAUSE 10··································· ' minimum period between pulses
··· NEXT
··· RETURN
Backup:·········································· 'Backup
····· HIGH 10
····· HIGH 2
····· pulseLeft = 650
····· pulseRight = 850
··· RETURN
Left_Turn:······································· ' Turn Left
····· HIGH 10
····· pulseLeft = 850
····· pulseRight = 850
··· RETURN
Right_Turn:······································ ' Turn Right
····· HIGH 2
····· pulseLeft = 650
····· pulseRight = 650
··· RETURN
Forward:········································· ' Forward
····· LOW 2
····· LOW 10
····· pulseLeft = 850
····· pulseRight = 650
··· RETURN
RJ_Print:········································ ' right justify
····· digits = 5
····· LOOKDOWN temp, <[noparse][[/noparse]0,10,100,1000,65535], digits
····· DEBUG REP " "\(5 - digits), DEC temp
··· RETURN

Do you see anywhere else I might smooth out the servo's? I would like to keep the distance reporting back to the computer.

Chris Savage

One thing that will prevent the code from ever being as fast as the IR Roaming is that light travels much faster than sound.· It also processes a little faster because of the way it's being read back into the BASIC Stamp.· On the other hand, I found I sped my PING))) BOE-Bot up by only pinging once every 20 iterations of the loop that refreshes the servos.· That made my BOE-Bot go from jittery to smooth very easily.

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