private bool DistanceHeading(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { Delta = Math.Abs(dc.target) - Vector3.Distance(RoverOrigPos, mVessel.CoM); DeltaT = Delta / RoverSpeed; if (Delta > 0) { fs.wheelThrottle = (float)throttlePID.Update(RoverSpeed, dc.speed, -1.0, 1.0); if (ForwardAxis != Vector3.zero) { Vector3d actuation = pidController.GetActuation(targetRotation); float steeringOutput = (float)-steerPID.Update(RTUtil.AngleBetween(RoverHDG, dc.target2), 0, -1.0, 1.0); if (mVessel.radarAltitude > minRadarAlt) { fs.pitch = Mathf.Clamp((float)actuation.x, -driveLimit, driveLimit); fs.roll = Mathf.Clamp((float)actuation.y, -driveLimit, driveLimit); } fs.yaw = Mathf.Clamp((float)actuation.z, -driveLimit, driveLimit); //keep if u want jet car fs.wheelSteer = SmoothenWheelSteering(RTUtil.AngleBetween(RoverHDG, dc.target2), steeringOutput, -dc.steering, dc.steering); } return(false); } else { fs.wheelThrottle = 0; fs.wheelSteer = 0; mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); dc.mode = RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Off; return(true); } }
public void OnFixedUpdate() { if (Vessel != null) { UpdateStateVectors(Vessel, Target); SteeringHelper.AnalyzeParts(Vessel); Vector3 Torque = SteeringHelper.TorqueAvailable; var CoM = Vessel.CoM; var MoI = Vessel.MOI; phiTotal = calculatePhiTotal(); phiVector = calculatePhiVector();//deviation errors from orientation target for (int i = 0; i < 3; i++) { MaxOmega[i] = Mathf.Max(Torque[i] / MoI[i], 0.0001f); } TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]); TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]); TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]); if (Math.Abs(phiTotal) > RollControlRange) { TargetOmega[1] = 0; rollRatePI.ResetI(); } TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], Vessel.MOI[0], Torque[0]); TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]); TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]); } }
public void OnFixedUpdate() { if (Vessel != null) { UpdateStateVectors(Vessel, Target); Vector3 Torque = SteeringHelper.GetVesselTorque(Vessel); var CoM = Vessel.CoM; var MoI = Vessel.MOI; phiTotal = PhiTotal(); phiVector = PhiVector(); for (int i = 0; i < 3; i++) { MaxOmega[i] = Torque[i] * MaxStoppingTime / MoI[i]; } TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]); TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]); TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]); if (Math.Abs(phiTotal) > RollControlRange) { TargetOmega[1] = 0; rollRatePI.ResetI(); } TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], Vessel.MOI[0], Torque[0]); TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]); TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]); } }
private bool Turn(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { Delta = Math.Abs(Quaternion.Angle(mRoverRot, mVessel.ReferenceTransform.rotation)); DeltaT = Delta / mVessel.GetComponent <Rigidbody>().angularVelocity.magnitude; if (Delta < dc.target) { fs.wheelThrottle = (float)throttlePID.Update(RoverSpeed, dc.speed, -1.0, 1.0); fs.wheelSteer = dc.steering; return(false); } else { fs.wheelThrottle = 0; fs.wheelSteer = 0; mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true); dc.mode = RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Off; return(true); } }
public void OnFixedUpdate() { if (Vessel != null) { UpdateStateVectors(Vessel, Target); SteeringHelper.AnalyzeParts(Vessel); Vector3 Torque = SteeringHelper.TorqueAvailable; var CoM = Vessel.CoM; var MoI = Vessel.MOI; phiTotal = calculatePhiTotal(); phiVector = calculatePhiVector();//deviation errors from orientation target for (int i = 0; i < 3; i++) { //Edge case: Very low (torque/MoI) (like 0.0078!) rate so need to rise max acceleration artifically StoppingTime = (OmegaThreshold <= (Torque[i] / MoI[i])) ? 1.0f : (float)RTUtil.Clamp((1.0 / (Torque[i] / MoI[i])) * (Math.Abs(phiVector[i]) - Phi1FStoppingTime), 1.0, MaxStoppingTime); MaxOmega[i] = Mathf.Max((Torque[i] * StoppingTime) / MoI[i], 0.0001f); } TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]); TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]); TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]); if (Math.Abs(phiTotal) > RollControlRange) { TargetOmega[1] = 0; rollRatePI.ResetI(); } TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], Vessel.MOI[0], Torque[0]); TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]); TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]); } }