con
driveServoL = 1
driveServoR = 2
neckServo =  3
panServo = 4
PingSigPin = 5
driveServostop = 1563
neckCentre = 1500
panCentre = 1500
danger = 12

_clkmode = xtal1 + pll4x
var
long distance
long leftDistance
long rightDistance
obj
servo: "Servo32v5.spin"     
ping: "Ping.spin"
Pub main
servo.Start
servo.Set(neckServo, neckCentre)
servo.Set(panServo, panCentre)
servo.Set(driveServoL, driveServostop)
servo.Set(driveServoR, driveServostop)

drive
pub drive
repeat
distance = centimeters(PingSigPin)
if distance < danger
  ! scan
  else
    servo.SetRamp(driveServoL, 863, 150)
    servo.SetRamp(driveServoR, 2263,150)
pub scan
servo.Set(panServo, 900)
leftDistance = centimeters(PingSigPin)
servo.set(panServo, 2100)
rightDistance = centimetres(PingSigPin)
if leftdistance < rightdistance
  !turnright
  else
    turnleft
pub turnright
servo.Set(driveServoL, driveServostop)
servo.Set(driveServoR, driveServostop)
waitcnt(500_000 + cnt)
servo.Set(driveServoL, 863)
waitcnt(500_000 + cnt)
servo.set(driveServoL, driveServostop)
drive
pub turnleft
servo.Set(driveServoL, driveServostop)
servo.Set(driveServoR, driveServostop)
waitcnt(500_000 + cnt)
servo.Set(driveServoR, 863)
waitcnt(500_000 + cnt)
servo.set(driveServoR, driveServostop)