コード例 #1
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(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;
            }
        }
コード例 #2
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;
            }
        }