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
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