rover

drive_to.ks

DECLARE PARAMETER waypointName.
DECLARE PARAMETER withinRadius.

SET w TO WAYPOINT(waypointName).

print("--- ROVER DRIVE TO PROGRAM ---").
print("[INFO] Waypoint: " + waypointName).
print("[INFO] Distance: " + w:GEOPOSITION:DISTANCE + "m").
print("[INFO] ETA: " + w:GEOPOSITION:DISTANCE / 4.0 + "s").

print("[INFO] Dis-engaging brakes...").
SAS on.
BRAKES off.

print("[INFO] Navigation started").

SET marker TO VECDRAW().
SET marker:COLOR TO RGB(0.6, 0.0, 0.0).
SET marker:LABEL TO "Rover Waypoint: " + waypointName.
SET marker:START TO SHIP:POSITION.
SET marker:VEC TO w:POSITION.
SET marker:WIDTH TO 0.15.
SET marker:SHOW TO true.

SET debug TO VECDRAW().
SET debug:COLOR TO RGB(0.0, 0.0, 0.6).
SET debug:START TO SHIP:POSITION.
SET debug:SHOW TO true.

SET front TO SHIP:PARTSTAGGED("front")[0].

LOCK STEERING TO HEADING(w:GEOPOSITION:HEADING, 0).
SET MAX_SPEED TO 4.

until w:GEOPOSITION:DISTANCE < withinRadius {

    SET marker:VEC TO w:POSITION.
    SET debug:VEC TO front:FACING * V(0.0, 0.0, 1.0).

    SET diff TO VECTORANGLE(debug:VEC, w:POSITION).

    if diff > 45 {
        SET MAX_SPEED TO 1.

    } else if diff > 20 OR w:GEOPOSITION:DISTANCE < withinRadius * 1.5 {
        SET MAX_SPEED TO 2.

    } else {
        SET MAX_SPEED TO 4.
    }.

    if GROUNDSPEED > MAX_SPEED {LOCK WHEELTHROTTLE TO -0.05.}.
    if GROUNDSPEED < MAX_SPEED {LOCK WHEELTHROTTLE TO 0.15.}.
    if GROUNDSPEED < MAX_SPEED * 0.55 {LOCK WHEELTHROTTLE TO 1.0.}.
    if GROUNDSPEED > MAX_SPEED * 1.025 {LOCK WHEELTHROTTLE TO -0.5.}.
    if GROUNDSPEED > MAX_SPEED * 1.25 {LOCK WHEELTHROTTLE TO -1.0.}.
    if GROUNDSPEED > MAX_SPEED * 1.75 {LOCK WHEELTHROTTLE TO -1.0.}.

    if SHIP:ELECTRICCHARGE < 100 {
        break.
    }.

}

SET marker:SHOW TO false.
SET debug:SHOW TO false.

if SHIP:ELECTRICCHARGE < 100 {
    print("[WARN] Critical electric charge reached!").
    print("[FAILURE] Navigation aborted!").

} else {
    print("[INFO] Target reached, stopping...").
    LOCK WHEELTHROTTLE TO -0.5.
    WAIT UNTIL GROUNDSPEED < 2.
    print("[SUCCESS] Navigation completed.").
}.

BRAKES on.
UNLOCK ALL.