Hcsr04 on a servo bot

stan74

Senior Member
I modded my 1st bot to use 1 hcrs04 on a servo instead of 3 stationary sensors.It works better..less missed obstacles.
I don't want to mod the servo pot to get more rotation..it's tiny.
What to do with 3 hcrs04's. Here's a poor video.Not much room but it didn't crash.The code needs tweaking.
https://youtu.be/-MT9xMkoBm0

Code:
init:
symbol lpingecho = B.1
symbol rpingecho = B.3
symbol servopin = C.4
symbol pingecho = C.5
symbol lmotordir = B.6
symbol rmotordir = B.7
symbol radarpos = b2
symbol radardir = b3
symbol range = w3
symbol saferange = w4
let saferange = 420
let radarpos = 150 ; mid 225 left 75 right
let radardir = 15

pulsout servopin,radarpos
pause 5000
;for b0 = 1 to 60
;	gosub rotateradar
;	pause 100
;next b0

pwmout pwmdiv16, C.1,124,0 ; 1000Hz @  8MHz left motor 
pwmout pwmdiv16, C.2,124,0 ; 1000Hz @  8MHz right motor
let w5 = timer ; random
main: ;----------------------- START ------------------
high lmotordir ; motors
high rmotordir ; forward
gosub motors_fullspeed

gosub rotateradar

;set saferange range for different directions
if radarpos > 119 and radarpos < 181 then
	let saferange = 420 ; front
else  let saferange = 500 ; left and right
endif

;test for obstacles
ping:
high pingecho 
pulsin pingecho,1,range
pause 10
if range = 0 then
	goto ping
endif
if range > saferange then ;---- ahead is clear
	goto main
endif

;ahead blocked
gosub brake
gosub motors_stop

if radarpos > 179 then ;--- blocked left
	gosub turnright	
elseif radarpos < 121 then ;--- blocked right	
	gosub turnleft
endif	
	
;---- ahead blocked so random test turn left or right
randomturn:
random w5
;sertxd("random= ",#b10,13,10)

if b10 < 128 then
	pulsout servopin,75 ;---- point radar right
	high pingecho ;---- test if right is clear
	pulsin pingecho,1,range
	pause 9
	if range > saferange then ;---- right is clear
		gosub turnright
		goto main
	else
		gosub turnleft
		goto main
	endif
else pulsout servopin,225 ;---- point radar left
	high pingecho ;---- test if left is clear
	pulsin pingecho,1,range
	pause 9
	if range > saferange then ;---- left is clear
		gosub turnleft
		goto main
	else
		gosub turnright
		goto main
	endif
endif 
;......................................................
turnright:
let saferange = 500
pulsout servopin,190 ;---- point radar left
gosub motors_slow

do
	gosub scanleft_rotateright
loop until range > saferange ;---- spin clockwise until no object to left

pulsout servopin,225 ;---- point radar full left

gosub motors_stop
;let saferange = 550

do
	gosub scanleft_rotateright	
loop until range > saferange ;---- spin clockwise until no object to full left

gosub motors_slow
pause 50
gosub motors_stop
let radarpos = 150
pulsout servopin,radarpos ;---- point radar ahead

return
turnleft:
let saferange = 500
pulsout servopin,110 ;----point radar right
gosub motors_slow

do
	gosub scanright_rotateleft
loop until range > saferange ; ---- spin anti-clockwise until no object to right

gosub motors_stop
;let saferange = 550

pulsout servopin,75 ;---- point radar full right

do
	gosub scanright_rotateleft
loop until range > saferange ; ---- spin anti-clockwise until no object to full right

gosub motors_slow
pause 50
gosub motors_stop
let radarpos = 150
pulsout servopin,radarpos ;---- point radar ahead

return
scanright_rotateleft:
high pingecho 
pulsin pingecho,1,range
gosub rotate_anticlockwise
pause 10
gosub motors_stop
;pause 5
return
scanleft_rotateright:
high pingecho 
pulsin pingecho,1,range
gosub rotate_clockwise
pause 10
gosub motors_stop
;pause 5
return
rotateradar:
pulsout servopin,radarpos                            ;--- rotate radar
let radarpos = radarpos + radardir                   ;
if radarpos = 225 then let radardir = 241 ;190 225   ;
elseif radarpos = 75 then let radardir = 15 ;110 75  ;
endif
return
brake:
low lmotordir ; short reverse
low rmotordir
pause 7
return
motors_stop:
pwmduty C.1,0 ; motors
pwmduty C.2,0 ;  off
return
motors_fullspeed:
pwmduty C.1,499 ; motors
pwmduty C.2,499 ;  full
return
motors_slow:
pwmduty C.1,249 ; motors
pwmduty C.2,249 ;  50%
return
rotate_clockwise:
high lmotordir ; left motor forward
low rmotordir ; right motor reverse
return
rotate_anticlockwise:
low lmotordir ; left motor reverse
high rmotordir ; right motor forward
return
motors_forward:
high lmotordir ; left motor forward
high rmotordir ; right motor forward
return
 

stan74

Senior Member
Thanks erco.Inspired in part by your efforts and suggestion.
1st time I've used a servo.Aren't they clever? You know their position.I must try using interrupts to control one.
I'll use a dip switch to select different modes like your puppy routine,wall follower,keep going left.. or right.
US sensors have a narrow beam and several sensors wasn't as good as scanning with a servo in steps of 15.. 10 positions.. change position every program loop. Light seeking and avoidance simultaneously next.
 
Top