Autopilot Tesla V2

set done to false.
until done {
  clearscreen.
  main().

  function main {
    print "Welcome to Tesla AutoPilot V2!".
    print "1 - Drive to Target with targeted speed".
    print "2 - Call a car to you".
    print "Press 9 for exit".
    on ag9 {
      print "Goodbye!".
    }

    wait until ag1 or ag2.
    if ag1 {
      if brakes {
        toggle brakes.
      } else {
        print "There isnt brakes!".
      }

      drive().
    } else {
      if brakes {
        toggle brakes.
      } else {
        print "There isnt brakes!".
      }

      call().
    }
  }


  function tpid {
    set throttp to pidloop(1, 1, 1, -1, 1).
    set throttp:setpoint to ts.
    set thrott to throttp:update(time:seconds, ship:groundspeed).
  }

  function drive {
    set ts to 10.
    print "Going".
    set tp to latlng(0.2,-74.7).
    lock distancelng to (-tp:lng - -ship:geoposition:lng) * 10452.
    lock distancelat to (-tp:lat - -ship:geoposition:lat) * 10452.
    if tp:lng > ship:geoposition:lng {
      lock distancelng to (-ship:geoposition:lng - -tp:lng) * 10452.
    }

    if tp:lat > ship:geoposition:lat {
      lock distancelat to (-ship:geoposition:lat - -tp:lat) * 10452.
    }

    lock distance to tp:distance.
    until distance < 20 {
      print distance.
      tpid().
      lock wheelthrottle to thrott.
      lock wheelsteering to tp.
    }

    toggle brakes.
    lock wheelthrottle to 0.
    wait until ship:groundspeed < 1.
    unlock wheelsteering.
    unlock wheelthrottle.
    print "Done".
    toggle ag1.
    main().
  }

  function call {
    set ts to 5.
    set k to vessel("Валентина Керман"):geoposition.
    lock distancelng to (-k:lng - -ship:geoposition:lng) * 10452.
    lock distancelat to (-k:lat - -ship:geoposition:lat) * 10452.
    if k:lng > ship:geoposition:lng {
      lock distancelng to (-ship:geoposition:lng - -k:lng) * 10452.
    }

    if k:lat > ship:geoposition:lat {
      lock distancelat to (-ship:geoposition:lat - -k:lat) * 10452.
    }

    lock distance to k:distance.
    until distance < 10 {
      print distance.
      tpid().
      lock wheelthrottle to thrott.
      lock wheelsteering to k.
    }

    toggle brakes.
    lock wheelthrottle to 0.
    wait until ship:groundspeed < 1.
    unlock wheelsteering.
    unlock wheelthrottle.
    print "Done".
    toggle ag2.
    main().
  }
}