FIX SCRIPT

//This is for the JVO Bushwhacker 4x4.
//------------------------------------
//(S) = Settable is something you can set. (C) = Caution is something you can set, but shouldn't. (N) = Not Settable is calculated and dynamic.
//------------------------------------

//Front Wheel Management
//Front Wheel Speed Limiter
set wheeldiameter to 4.1. //(C) Meters.
set wheelcircumference to (2*constant:pi)*(wheeldiameter/2). //(C) Circumference.
set frontwheelrpmminimum to 20. //(S) This makes it so it's never 0, otherwise you can't take off.
set frontwheelrpmlimitmultiplier to 120. //(S) This is how much faster the wheel should go. Front wheels spin faster, and that helps with stability.
set frontwheelrpmlimit to (groundspeed/wheelcircumference)*60. //(N) Setting the rpm limit for the first time, but at 0 speed, this is 0.

//Steering
set steer to 0. // (N) This is the angle for all hinges.
set yawsteer to 0. //(N) This is Yaw Control for the reaction wheels.
set yawmultiplier to .5. //(S) Yaw Control Multiplier. This makes Yaw help us steer.
set steeringmultiplier to 1.25. //(S) Steering Multiplier. This can keep you from wandering.
set dz to 5. //(S) This is the Deadzone. If Desired Heading is within this number of degrees, it'll steer straight.
set lim to 30. //(S) Steering Angle Clamp.

//Heading Management
set tar to body:geopositionlatlng(0,0). //(S) Target to lock to with HDGLOCK.
set hdg to 0. //(S) This is the Heading input.
set hdgtrim to 0. //(S) Use this to navigate around obstacles without changing HDG or TAR values unnecessarily.
set hdglock to false. //(S) Heading lock.
set inverted to false. //(S) Turns off steering when flipped, to aid in not going all over the place.
set inair to 5. //(S) This tells you whether it's off the ground or not. Probably should change that.

lock trueheading to body:geopositionof(ship:facing:forevector):heading. //(N) This is True Heading. By: numberless_tim
set p to 0. //Pitch. This should be checked, maybe pitched up to 10 or something on smaller planets.

//Anomaly Loader
set as to addons:scansat:getanomalies(body).

//Rear Wheel Management
set maxrpm to 200. //(S) Maximum Wheel RPM for Rear Wheels. Front wheel RPM is managed by Groundspeed.
//Torque Limit % Management for all 4 wheels
set maxtorque to 40. //(S) Maximum Torque % at full throttle (Max RPM). Anything above 40 causes Brownout.
set torquelim to 0. // (N) "Torque Limit %" value sent to Drive Rotors.
set maxtorquesnap to 80. //(S) Maximum Torque % that gets snapped to 100% for high power, low speed driving.
set minthrottletorquecutoff to .01. //(S) Minimum Throttle Level for Torque Cutoff. Below this, Torque % is 0, to allow for freewheeling.

//Spinny Things On.
ship:partstagged("MR1")[0]:getmodule("ModuleRoboticServoRotor"):setfield("torque limit(%)",10). //This is the spinny thing in the middle.
ship:partstagged("MR2")[0]:getmodule("ModuleRoboticServoRotor"):setfield("torque limit(%)",10). //It spins.

//Steering Stuff.
//Per Physics Tick
on time:seconds {
    if hdglock {set hdg to tar:heading+hdgtrim.}.
    if not inverted {set steer to ((body:geopositionof(heading(hdg,0):vector):bearing)*steeringmultiplier)+hdgtrim.} else {set steer to 0.}.
    if steer > lim {set steer to lim.}.
    if steer < -lim {set steer to -lim.}.
    lock steering to srfprograde.
    if (abs(steer) > dz) {set ship:control:yaw to (-(-steer/steer)*(steer/lim))*yawmultiplier.} else {set ship:control:yaw to 0.06.}.
    if (inair < alt:radar) {set ship:control:yaw to 0. lock steering to srfprograde.}.

    //Set Front Hinges.
    for hinges in ship:partsdubbedpattern("hinge") {hinges:getmodule("ModuleRoboticServoHinge"):setfield("target angle",steer*steeringmultiplier)}.

    //Front Wheel Management
    set frontwheelrpmlimit to ((groundspeed/wheelcircumference)*60)*(frontwheelrpmlimitmultiplier/100).
    ship:partstagged("LFR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("rpm limit",max(frontwheelrpmminimum,frontwheelrpmlimit)).
    ship:partstagged("RFR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("rpm limit",max(frontwheelrpmminimum,frontwheelrpmlimit)).
    return true.
}

//End of Physics Tick Loop

//Throttle Trigger.
on throttle {
    set torquelim to max(100-(throttle*100),maxtorque).
    if (torquelim > maxtorquesnap) {set torquelim to 100}
    if (throttle < minthrottletorquecutoff) {set torquelim to 0}
    ship:partstagged("LFR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("torque limit(%)",torquelim).
    ship:partstagged("RFR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("torque limit(%)",torquelim).
    ship:partstagged("LRR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("torque limit(%)",torquelim).
    ship:partstagged("RRR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("torque limit(%)",torquelim).
    ship:partstagged("LRR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("rpm limit",maxrpm*throttle).
    ship:partstagged("RRR")[0]:getmodule("ModuleRoboticServoRotor"):setfield("rpm limit",maxrpm*throttle).
    return true.
}

//End of Throttle Trigger.

wait 5. //This is for the display. It does weird things like tell you the throttle doesn't exist if you don't wait.

//Make tiny textbox with speed in KPH and MPH.
set displayWid to gui(150).
displayWid:show().
    set mphSpeed to displayWid:addlabel("mph").
    set kmhSpeed to displayWid:addlabel("kmh").
    set ktsSpeed to displayWid:addlabel("kts").
    set truHeading to displayWid:addlabel("truHeading").
    set hdgdisplay to displayWid:addlabel("heading").
    set hdgtrimdisplay to displayWid:addlabel("trim").
    set throttledisplay to displayWid:addlabel("throttle").
    set displayWid:x to -64.
    set displayWid:y to -450.
    set keep to true.

on time:seconds {
    set mphSpeed:text to "<color=white><b>Speed: "+round((velocity:surface:mag*2.23694),2):tostring+" MPH</b></color>".
    set kmhSpeed:text to "<color=white><b>Speed: "+round((velocity:surface:mag*3.6),2):tostring+" KMH</b></color>".
    set ktsSpeed:text to "<color=white><b>Speed: "+round((velocity:surface:mag*1.94384),2):tostring+" KTS</b></color>".
    set throttledisplay:text to "<color=white><b>Throttle Pos: "+round((throttle),2):tostring+"</b></color>".
    set truHeading:text to "<color=white><b>True Heading: "+round(trueheading,2):tostring+"deg.</b></color>".
    set hdgdisplay:text to "<color=white><b>Heading: "+round(hdg,2):tostring+"deg.</b></color>".
    set hdgtrimdisplay:text to "<color=white><b>Heading Trim: "+hdgtrim+"deg.</b></color>".
    return keep.
}

//Put this stuff into a widget already.
set posdata to true.
on time:second {
    print "<color>Lat: "+round(ship:latitude,2)+"</color>" at (0,0).
    print "<color>Lng: "+round(ship:longitude,2)+"</color>" at (0,1).
    print "<color>Biome: "+addons:scansat:currentbiome+"</color>" at (0,2).
    print "<color>Dis: "+round(tar:distance)+"</color>" at (0,3).
    print "<color>Time: "+round(tar:distance/groundspeed/60)+"m</color>" at (0,4).
    print "<color>EC: "+round(ship:electriccharge)+"</color>" at (0,5).
    return posdata.
}