[color=Green];ultrasonic on servo robot
;pins c.1 and c.2 were pwm.Now turns motors on/off.
;pins b.6 and b.7 1=forward 0=reverse[/color]
[color=Black]init:[/color]
[color=Navy]#picaxe [/color][color=Black]28x2[/color]
[color=Blue]symbol [/color][color=Black]servopin [/color][color=DarkCyan]= [/color][color=Blue]B.5 [/color][color=Green];servo pin[/color]
[color=Blue]symbol [/color][color=Black]pingecho [/color][color=DarkCyan]= [/color][color=Blue]C.5
symbol [/color][color=Black]lmotordir [/color][color=DarkCyan]= [/color][color=Blue]B.6
symbol [/color][color=Black]rmotordir [/color][color=DarkCyan]= [/color][color=Blue]B.7
symbol [/color][color=Black]radarpos [/color][color=DarkCyan]= [/color][color=Purple]b2 [/color]
[color=Blue]symbol [/color][color=Black]radardir [/color][color=DarkCyan]= [/color][color=Purple]b3[/color]
[color=Blue]symbol [/color][color=Black]range [/color][color=DarkCyan]= [/color][color=Purple]w3[/color]
[color=Blue]symbol [/color][color=Black]saferange [/color][color=DarkCyan]= [/color][color=Purple]w4[/color]
[color=Blue]let [/color][color=Black]saferange [/color][color=DarkCyan]= [/color][color=Navy]420[/color]
[color=Blue]let [/color][color=Black]radarpos [/color][color=DarkCyan]= [/color][color=Navy]150 [/color][color=Green]; mid 75 left 225 right[/color]
[color=Blue]let [/color][color=Black]radardir [/color][color=DarkCyan]= [/color][color=Navy]1[/color]
[color=Blue]servo [/color][color=Black]servopin,[/color][color=Navy]150 [/color][color=Green];---- point radar ahead[/color]
[color=Blue]pause [/color][color=Navy]5000[/color]
[color=Blue]for [/color][color=Purple]b0 [/color][color=DarkCyan]= [/color][color=Navy]1 [/color][color=Blue]to [/color][color=Navy]60
[/color][color=Blue]gosub [/color][color=Black]rotateradar[/color]
[color=Blue]next [/color][color=Purple]b0[/color]
[color=Blue]let [/color][color=Purple]w5 [/color][color=DarkCyan]= [/color][color=Purple]timer [/color][color=Green]; random[/color]
[color=Black]main: [/color][color=Green];----------------------- START ------------------[/color]
[color=Blue]high c.1[/color][color=Black],[/color][color=Blue]c.2[/color][color=Black]:[/color][color=Blue]high [/color][color=Black]rmotordir:[/color][color=Blue]high [/color][color=Black]lmotordir [/color][color=Green]; motors forward[/color]
[color=Blue]gosub [/color][color=Black]rotateradar
[/color]
[color=Green];---- set saferange for different directions[/color]
[color=Blue]if [/color][color=Black]radarpos [/color][color=DarkCyan]>= [/color][color=Navy]130 [/color][color=Blue]then
if [/color][color=Black]radarpos [/color][color=DarkCyan]<= [/color][color=Navy]170 [/color][color=Blue]then
let [/color][color=Black]saferange [/color][color=DarkCyan]= [/color][color=Navy]430 [/color][color=Green]; front
[/color][color=Blue]endif
else let [/color][color=Black]saferange [/color][color=DarkCyan]= [/color][color=Navy]470 [/color][color=Green]; left and right[/color]
[color=Blue]endif[/color]
[color=Green];--- test for obstacles [/color]
[color=Blue]if [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Blue]then [/color][color=Green];---- ahead is clear
[/color][color=Blue]goto [/color][color=Black]main[/color]
[color=Blue]endif[/color]
[color=Green];ahead blocked[/color]
[color=Blue]gosub [/color][color=Black]brake[/color]
[color=Blue]if [/color][color=Black]radarpos [/color][color=DarkCyan]> [/color][color=Navy]170 [/color][color=Blue]then [/color][color=Green];--- blocked left
[/color][color=Blue]goto [/color][color=Black]turnright [/color]
[color=Blue]elseif [/color][color=Black]radarpos [/color][color=DarkCyan]< [/color][color=Navy]130 [/color][color=Blue]then [/color][color=Green];--- blocked right
[/color][color=Blue]goto [/color][color=Black]turnleft[/color]
[color=Blue]endif
[/color]
[color=Green];---- ahead blocked so random test turn left or right[/color]
[color=Blue]low [/color][color=Black]lmotordir,rmotordir[/color]
[color=Blue]pause [/color][color=Navy]15[/color]
[color=Blue]low c.1[/color][color=Black],[/color][color=Blue]c.2
random [/color][color=Purple]w5[/color]
[color=Blue]if [/color][color=Purple]b10 [/color][color=DarkCyan]< [/color][color=Navy]128 [/color][color=Blue]then
servopos [/color][color=Black]servopin,[/color][color=Navy]100 [/color][color=Green];point radar far right
[/color][color=Blue]gosub [/color][color=Black]ping
[/color][color=Blue]if [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Blue]then [/color][color=Green];---- right is clear
[/color][color=Blue]goto [/color][color=Black]turnright
[/color][color=Blue]else
goto [/color][color=Black]turnleft
[/color][color=Blue]endif
else servopos [/color][color=Black]servopin,[/color][color=Navy]200 [/color][color=Green];---- point radar far left
[/color][color=Blue]gosub [/color][color=Black]ping
[/color][color=Blue]if [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Blue]then [/color][color=Green];---- left is clear
[/color][color=Blue]goto [/color][color=Black]turnleft
[/color][color=Blue]else
goto [/color][color=Black]turnright
[/color][color=Blue]endif
endif [/color]
[color=Green];......................................................[/color]
[color=Black]turnright:[/color]
[color=Blue]high [/color][color=Black]lmotordir:[/color][color=Blue]low [/color][color=Black]rmotordir:[/color][color=Blue]let [/color][color=Black]saferange [/color][color=DarkCyan]= [/color][color=Navy]470[/color][color=Black]:[/color][color=Blue]servopos [/color][color=Black]servopin,[/color][color=Navy]170 [/color][color=Green];---- point radar left[/color]
[color=Blue]do[/color][color=Black]:[/color][color=Blue]gosub [/color][color=Black]scanrotateright:[/color][color=Blue]loop until [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange:[/color][color=Blue]servopos [/color][color=Black]servopin,[/color][color=Navy]170 [/color][color=Green];---- point radar more left[/color]
[color=Blue]do[/color][color=Black]:[/color][color=Blue]gosub [/color][color=Black]scanrotateright:[/color][color=Blue]loop until [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange:[/color][color=Blue]servopos [/color][color=Black]servopin,[/color][color=Navy]215 [/color][color=Green];---- point radar full left[/color]
[color=Blue]do[/color][color=Black]:[/color][color=Blue]gosub [/color][color=Black]scanrotateright:[/color][color=Blue]loop until [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Green];---- spin clockwise until no object to full left[/color]
[color=Blue]let [/color][color=Black]radarpos [/color][color=DarkCyan]= [/color][color=Navy]126[/color][color=Black]:[/color][color=Blue]let [/color][color=Black]radardir[/color][color=DarkCyan]=[/color][color=Navy]1[/color][color=Black]:[/color][color=Blue]servopos [/color][color=Black]servopin,radarpos [/color][color=Green];---- point radar right[/color]
[color=Blue]goto [/color][color=Black]main[/color]
[color=Green];......................................................[/color]
[color=Black]turnleft:[/color]
[color=Blue]low [/color][color=Black]lmotordir:[/color][color=Blue]high [/color][color=Black]rmotordir:[/color][color=Blue]let [/color][color=Black]saferange [/color][color=DarkCyan]= [/color][color=Navy]470[/color][color=Black]:[/color][color=Blue]servopos [/color][color=Black]servopin,[/color][color=Navy]150 [/color][color=Green];----point radar ahead[/color]
[color=Blue]do[/color][color=Black]:[/color][color=Blue]gosub [/color][color=Black]scanrotateleft:[/color][color=Blue]loop until [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Green]; ---- spin anti-clockwise until no object ahead[/color]
[color=Blue]servopos [/color][color=Black]servopin,[/color][color=Navy]110 [/color][color=Green];---- point radar right[/color]
[color=Blue]do[/color][color=Black]:[/color][color=Blue]gosub [/color][color=Black]scanrotateleft:[/color][color=Blue]loop until [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Green]; ---- spin anti-clockwise until no object to more right[/color]
[color=Blue]servopos [/color][color=Black]servopin,[/color][color=Navy]85 [/color][color=Green];---- point radar full right[/color]
[color=Blue]do[/color][color=Black]:[/color][color=Blue]gosub [/color][color=Black]scanrotateleft:[/color][color=Blue]loop until [/color][color=Black]range [/color][color=DarkCyan]> [/color][color=Black]saferange [/color][color=Green]; ---- spin anti-clockwise until no object to full right[/color]
[color=Blue]let [/color][color=Black]radarpos [/color][color=DarkCyan]= [/color][color=Navy]174[/color][color=Black]:[/color][color=Blue]let [/color][color=Black]radardir[/color][color=DarkCyan]=[/color][color=Navy]0[/color][color=Black]:[/color][color=Blue]servopos [/color][color=Black]servopin,radarpos [/color][color=Green];---- point radar left[/color]
[color=Blue]goto [/color][color=Black]main[/color]