ultras sonic on servo robot

[stan cartwright]

Here's code for the 18f25k22 to control an object avoiding robot. It was converted from picaxe basic, https://youtu.be/DZVkTLNGYs8 It uses timer 1 interrupt to run the servo.
;ultrasonic on servo robot

#chip 18F25K22, 16
#config OSC = INTIO67

;
Dim Servo_Pos as WORD
Dim TIME_BASE alias CCPR1H,CCPR1L AS WORD   '// Programmable "Compare" Value
CCP1CON = b'00001011'     '// Config CCP1 for Compare:Spcl Event trigger
;
TIME_BASE = 20000    '// Make period timer = 20ms (2000us)
;
On Interrupt CCP1 Call Servo_Pulse ;every 20ms
;
InitTimer1 OSC,PS1_4    '// Configure timer1 with Prescale of 1:4
ClearTimer 1            '//Make Sure timer is cleared to 0
StartTimer 1            '// Start the timer
;
#define Servo_Pin PORTb.7   '// Servo signal On PIN 23
#define echo = portB.1 ;ultrasonic sensor trig pin.echo pin joined to trig by 1.8k resistor
#define lmotordira = portA.0 ;h-bridge motor pins
#define lmotordirb = portA.1
#define rmotordira = portA.2
#define rmotordirb = portA.3
dir lmotordira out : dir lmotordirb out : dir rmotordira out : dir rmotordirb out
dir servo_pin out
[word]saferange = 420
radardir = 1
Servo_POS= 150 ;---- point radar ahead
wait 5000 ms
;
for tmp = 1 to 40
  gosub rotate_radar
  wait 100 ms
next
;
main: ;----------------------- START ------------------
gosub motors_forward
gosub rotate_radar
;
;---- set saferange for different directions
if radarpos >= 130 and radarpos <= 170 then
    saferange = 430 ; front
else
    saferange = 470 ; left and right
end if
;
;--- test for obstacles
if range > saferange then
  goto main
end if ;---- ahead is clear
;
;ahead blocked
gosub brake
if radarpos > 170 then ;--- blocked left
  goto turnright
else
  if radarpos < 130 then ;--- blocked right
  goto turnleft
end if
;
;---- ahead blocked so random test turn left or right
tmp = Random
if tmp < 128 then
  servo_pos=100 ;point radar far right
  gosub ping
  if range > saferange then ;---- right is clear
    goto turnright
  else
    goto turnleft
  end if
else
  servo_pos=200 ;---- point radar far left
  gosub ping
  if range > saferange then ;---- left is clear
    goto turnleft
  else
    goto turnright
  end if
end if
;......................................................
turnright:
saferange = 470:servo_pos=150 ;---- point radar ahead
;
do:gosub scanrotateright:loop until range > saferange
servo_pos=170 ;---- point radar more left
;
do:gosub scanrotateright:loop until range > saferange
servo_pos=215 ;---- point radar full left
;
do:gosub scanrotateright:loop until range > saferange ;---- spin clockwise until no object to full left
;
radardir=1:servo_pos=126 ;---- point radar right
goto main
;......................................................
turnleft:
saferange = 470:servo_pos=150 ;----point radar ahead
;
do:gosub scanrotateleft:loop until range > saferange ; ---- spin anti-clockwise until no object ahead
servo_pos=110 ;---- point radar right
;
do:gosub scanrotateleft:loop until range > saferange ; ---- spin anti-clockwise until no object to more right
servo_pos=85 ;---- point radar full right
;
do:gosub scanrotateleft:loop until range > saferange ; ---- spin anti-clockwise until no object to full right
;
radardir=0:servo_pos=174 ;---- point radar left
goto main
;~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
sub Servo_Pulse
   PulseOut Servo_Pin, Servo_Pos us
end sub
scanrotateleft:
gosub ping
gosub rotate_anticlockwise
wait 7 ms
gosub motors_off
return
scanrotateright:
gosub ping
gosub rotate_clockwise
wait 7 ms
gosub motors_off
return
brake:
gosub motors_reverse
wait 10 ms
gosub motors_off
return
rotate_clockwise:
set lmotordira on; left motor forward
set lmotordira off
set rmotordira off ; right motor backward
set rmotordira on
return
rotate_anticlockwise:
set lmotordira off; left motor backward
set lmotordira on
set rmotordira on ; right motor forward
set rmotordira off
return
motors_forward:
set lmotordira on; left motor forward
set lmotordira off
set rmotordira on ; right motor forward
set rmotordira off
return
motors_off:
set lmotordira off; left motor off
set lmotordira off
set rmotordira off ; right motor off
set rmotordira off
return
motors_reverse:
set lmotordira off; left motor backward
set lmotordira on
set rmotordira off ; right motor backward
set rmotordira on
return
rotate_radar:
gosub ping
;
if radardir = 1 then
  servo_pos = radarpos +8
  if servo_pos = 222 then
    radardir = 0
  end if
else
  servo_pos = radarpos - 8
  if servo_pos = 78 then
    radardir = 1
  end if
end if
return
ping:
retry:
dir echo out
set echo on
dir echo in
pulsein echo, range , 1 ms
wait 10 ms
if range = 0 then
  goto retry
end if ;----out of range
return

[Anobium]

Great job. Is the video Great Cow BASIC or Picaxe?

[stan cartwright]

Picaxe basic,sorry. I'll post later. It's a "learn GCB" project, Commands have different names to picaxe. The robot code works better than some I've seen. GBC speed is not needed as there are 10ms pauses in the code to allow the US sensor to settle.