public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
{ // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
if (orbit.referenceBody != lastBody)
{
WaypointIndex = -1; Waypoints.Clear();
}
MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);
var brake = vessel.ActionGroups[KSPActionGroup.Brakes]; // keep brakes locked if they are
curSpeed = Vector3d.Dot(vessel.srf_velocity, vesselState.forward);
CalculateTraction();
speedIntAcc = speedPID.intAccum;
if (wp != null && wp.Body == orbit.referenceBody)
{
if (ControlHeading)
{
heading = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
}
if (ControlSpeed)
{
var nextWP = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
var distance = Vector3.Distance(vessel.CoM, wp.Position);
if (wp.Target != null)
{
distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
}
//var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed); // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
(nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
(distance - wp.Radius > 50 ? turnSpeed.val : 1)));
minSpeed = (wp.Quicksave ? 1 : minSpeed);
// ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
var brakeFactor = Math.Max((curSpeed - minSpeed) * 1, 3);
var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / brakeFactor, minSpeed)); // brake when getting closer
newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed); // reduce speed when turning a lot
if (LimitAcceleration)
{
newSpeed = curSpeed + Mathf.Clamp((float)(newSpeed - curSpeed), -1.5f, 0.5f);
}
// newSpeed = tgtSpeed + Mathf.Clamp((float)(newSpeed - tgtSpeed), -Time.deltaTime * 8f, Time.deltaTime * 2f);
var radius = Math.Max(wp.Radius, 10);
if (distance < radius)
{
if (WaypointIndex + 1 >= Waypoints.Count) // last waypoint
{
newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
// ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
if (LoopWaypoints)
{
WaypointIndex = 0;
}
else
{
newSpeed = 0;
// tgtSpeed.force(newSpeed);
if (curSpeed < brakeSpeedLimit)
{
if (wp.Quicksave)
{
//if (s.mainThrottle > 0) { s.mainThrottle = 0; }
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
QuickSaveLoad.QuickSave();
}
}
else
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
}
}
// else {
// Debug.Log("Is this even getting called?");
// WaypointIndex++;
// }
}
}
else
{
if (wp.Quicksave)
{
//if (s.mainThrottle > 0) { s.mainThrottle = 0; }
newSpeed = 0;
// tgtSpeed.force(newSpeed);
if (curSpeed < brakeSpeedLimit)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex++;
QuickSaveLoad.QuickSave();
}
}
}
else
{
WaypointIndex++;
}
}
}
brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
// ^ brake if needed to prevent rolling, hopefully
tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
}
}
if (ControlHeading)
{
headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
{
float spd = Mathf.Min((float)speed, (float)turnSpeed); // if a slower speed than the turnspeed is used also be more careful with the steering
float limit = (Mathf.Abs((float)curSpeed) <= turnSpeed ? 1 : Mathf.Clamp((float)(spd / Mathf.Abs((float)curSpeed)), 0.35f, 1f));
double act = headingPID.Compute(headingErr);
s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
}
}
// Brake if there is no controler (Pilot eject from seat)
if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
{
s.wheelThrottle = 0;
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
brake = true;
}
else if (ControlSpeed)
{
speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);
speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vessel.srf_velocity, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
{
float act = (float)speedPID.Compute(speedErr);
s.wheelThrottle = !LimitAcceleration?Mathf.Clamp((float)act, -1, 1) : // I think I'm using these ( ? : ) a bit too much
(traction == 0 ? 0 : (act < 0 ? Mathf.Clamp(act, -1f, 1f) : (lastThrottle + Mathf.Clamp(act - lastThrottle, -0.005f, 0.005f)) * (traction < tractionLimit ? -1 : 1)));
if (curSpeed < 0 & s.wheelThrottle < 0)
{
s.wheelThrottle = 0;
} // don't go backwards
// if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0) { s.wheelThrottle = Mathf.Clamp(act, -1f, 1f); }
if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) == 0) // StabilityControl && traction > 50 &&
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
{
brake = true;
}
// else if (!stabilityControl || traction <= 50 || speedErr > -0.2 || Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0) {
// vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, (GameSettings.BRAKES.GetKey() && vessel.isActiveVessel));
// }
lastThrottle = s.wheelThrottle;
}
}
if (StabilityControl)
{
if (!core.attitude.users.Contains(this))
{
core.attitude.users.Add(this);
// line.enabled = true;
}
// float scale = Vector3.Distance(FlightCamera.fetch.mainCamera.transform.position, vessel.CoM) / 900f;
// line.SetPosition(0, vessel.CoM);
// line.SetPosition(1, vessel.CoM + hit.normal * 5);
// line.SetWidth(0, scale + 0.1f);
var fSpeed = (float)curSpeed;
// if (Mathf.Abs(fSpeed) >= turnSpeed * 0.75) {
Vector3 fwd = (Vector3)(traction > 0 ? // V when the speed is low go for the vessels forward, else with a bit of velocity
// ((Mathf.Abs(fSpeed) <= turnSpeed ? vesselState.forward : vessel.srf_velocity / 4) - vessel.transform.right * s.wheelSteer) * Mathf.Sign(fSpeed) :
// // ^ and then add the steering
vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
vessel.srf_velocity); // in the air so follow velocity
Vector3.OrthoNormalize(ref norm, ref fwd);
var quat = Quaternion.LookRotation(fwd, norm);
// if (traction > 0 || speed <= turnSpeed) {
// var u = new Vector3(0, 1, 0);
//
// var q = FlightGlobals.ship_rotation;
// var q_s = quat;
//
// var q_u = new Quaternion(u.x, u.y, u.z, 0);
// var a = Quaternion.Dot(q, q_s * q_u);
// var q_qs = Quaternion.Dot(q, q_s);
// var b = (a == 0) ? Math.Sign(q_qs) : (q_qs / a);
// var g = b / Mathf.Sqrt((b * b) + 1);
// var gu = Mathf.Sqrt(1 - (g * g)) * u;
// var q_d = new Quaternion() { w = g, x = gu.x, y = gu.y, z = gu.z };
// var n = q_s * q_d;
//
// quat = n;
// }
core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
// }
}
if (BrakeOnEnergyDepletion)
{
var batteries = vessel.Parts.FindAll(p => p.Resources.Contains("ElectricCharge") && p.Resources["ElectricCharge"].flowState);
var energyLeft = batteries.Sum(p => p.Resources["ElectricCharge"].amount) / batteries.Sum(p => p.Resources["ElectricCharge"].maxAmount);
var openSolars = vessel.mainBody.atmosphere && // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.panelState != ModuleDeployableSolarPanel.panelStates.BROKEN &&
p.panelState != ModuleDeployableSolarPanel.panelStates.RETRACTED).Count > 0;
if (openSolars && energyLeft > 0.99)
{
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
p.panelState == ModuleDeployableSolarPanel.panelStates.EXTENDED).ForEach(p => p.Retract());
}
if (energyLeft < 0.05 && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0)
{
s.wheelThrottle = 0;
} // save remaining energy by not using it for acceleration
if (openSolars || energyLeft < 0.03)
{
tgtSpeed = 0;
}
if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
{
brake = true;
}
if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.panelState == ModuleDeployableSolarPanel.panelStates.EXTENDED).Count > 0)
{
waitingForDaylight = true;
}
}
// brake = brake && (s.wheelThrottle == 0); // release brake if the user or AP want to drive
if (s.wheelThrottle != 0 && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0)
{
brake = false; // the AP or user want to drive into the direction of momentum so release the brake
}
if (vessel.isActiveVessel)
{
if (GameSettings.BRAKES.GetKeyUp())
{
brake = false; // release the brakes if the user lets go of them
}
if (GameSettings.BRAKES.GetKey())
{
brake = true; // brake if the user brakes and we aren't about to flip
}
}
tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && curSpeed > brakeSpeedLimit ? traction >= tractionLimit : true));
// ^ brake but hopefully prevent flipping over, assuming the user set up the limit right
if (brake && curSpeed < 0.1)
{
s.wheelThrottle = 0;
}
}
public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
{ // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
if (orbit.referenceBody != lastBody)
{
WaypointIndex = -1; Waypoints.Clear();
}
MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);
var brake = vessel.ActionGroups[KSPActionGroup.Brakes]; // keep brakes locked if they are
curSpeed = Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
CalculateTraction();
speedIntAcc = speedPID.intAccum;
if (wp != null && wp.Body == orbit.referenceBody)
{
if (ControlHeading)
{
heading.val = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
}
if (ControlSpeed)
{
var nextWP = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
var distance = Vector3.Distance(vessel.CoM, wp.Position);
if (wp.Target != null)
{
distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
}
// var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed); // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
(nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
(distance - wp.Radius > 50 ? turnSpeed.val : 1)));
minSpeed = (wp.Quicksave ? 1 : minSpeed);
// ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / curSpeed, minSpeed)); // brake when getting closer
newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed); // reduce speed when turning a lot
var radius = Math.Max(wp.Radius, 10);
if (distance < radius)
{
if (WaypointIndex + 1 >= Waypoints.Count) // last waypoint
{
newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
// ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
if (LoopWaypoints)
{
WaypointIndex = 0;
}
else
{
newSpeed = 0;
brake = true;
if (curSpeed < brakeSpeedLimit)
{
if (wp.Quicksave)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
QuickSaveLoad.QuickSave();
}
}
else
{
WaypointIndex = -1;
ControlHeading = ControlSpeed = false;
}
}
}
}
else
{
if (wp.Quicksave)
{
newSpeed = 0;
if (curSpeed < brakeSpeedLimit)
{
if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
{
WaypointIndex++;
QuickSaveLoad.QuickSave();
}
}
}
else
{
WaypointIndex++;
}
}
}
brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
// ^ brake if needed to prevent rolling, hopefully
tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
}
}
if (ControlHeading)
{
headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);
double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
{
float limit = (Math.Abs(curSpeed) > turnSpeed ? Mathf.Clamp((float)((turnSpeed + 6) / Square(curSpeed)), 0.1f, 1f) : 1f);
// turnSpeed needs to be higher than curSpeed or it will never steer as much as it could even at 0.2m/s above it
double act = headingPID.Compute(headingErr);
if (traction >= tractionLimit)
{
s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
// prevents it from flying above a waypoint and landing with steering at max while still going fast
}
}
}
// Brake if there is no controler (Pilot eject from seat)
if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
{
s.wheelThrottle = 0;
brake = true;
}
else if (ControlSpeed)
{
speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);
speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
{
float act = (float)speedPID.Compute(speedErr);
s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
if (curSpeed < 0 & s.wheelThrottle < 0)
{
s.wheelThrottle = 0;
} // don't go backwards
if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0)
{
s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
}
if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Math.Sign(curSpeed) == 0)
{
brake = true;
}
lastThrottle = Mathf.Clamp(s.wheelThrottle, -1, 1);
}
}
if (StabilityControl)
{
if (!core.attitude.users.Contains(this))
{
core.attitude.users.Add(this);
}
var fSpeed = (float)curSpeed;
Vector3 fwd = (Vector3)(traction > 0 ? // V when the speed is low go for the vessels forward, else with a bit of velocity
vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
vesselState.surfaceVelocity); // in the air so follow velocity
Vector3.OrthoNormalize(ref norm, ref fwd);
var quat = Quaternion.LookRotation(fwd, norm);
if (vesselState.torqueAvailable.sqrMagnitude > 0)
{
core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
}
}
if (BrakeOnEnergyDepletion)
{
var batteries = vessel.Parts.FindAll(p => p.Resources.Contains(PartResourceLibrary.ElectricityHashcode) && p.Resources.Get(PartResourceLibrary.ElectricityHashcode).flowState);
var energyLeft = batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).amount) / batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).maxAmount);
var openSolars = vessel.mainBody.atmosphere && // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.deployState != ModuleDeployablePart.DeployState.BROKEN &&
p.deployState != ModuleDeployablePart.DeployState.RETRACTED).Count > 0;
if (openSolars && energyLeft > 0.99)
{
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
p.deployState == ModuleDeployablePart.DeployState.EXTENDED).ForEach(p => p.Retract());
}
if (energyLeft < 0.05 && Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0)
{
s.wheelThrottle = 0;
} // save remaining energy by not using it for acceleration
if (openSolars || energyLeft < 0.03)
{
tgtSpeed = 0;
}
if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
{
brake = true;
}
if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.deployState == ModuleDeployablePart.DeployState.EXTENDED).Count > 0)
{
waitingForDaylight = true;
}
}
if (s.wheelThrottle != 0 && (Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0 || curSpeed < 1))
{
brake = false; // the AP or user want to drive into the direction of momentum so release the brake
}
if (vessel.isActiveVessel)
{
if (GameSettings.BRAKES.GetKeyUp())
{
brake = false; // release the brakes if the user lets go of them
}
if (GameSettings.BRAKES.GetKey())
{
brake = true; // brake if the user brakes and we aren't about to flip
}
}
tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && (ControlHeading || ControlSpeed) ? traction >= tractionLimit : true));
// only let go of the brake when losing traction if the AP is driving, otherwise assume the player knows when to let go of it
// also to not constantly turn off the parking brake from going over a small bump
if (brake && curSpeed < 0.1)
{
s.wheelThrottle = 0;
}
}