private bool Coord(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { Delta = Vector3.Distance(mVessel.CoM, TargetPos); 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, TargetHDG), -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, TargetHDG), 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); } }
private bool Coord(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { float deg = RTUtil.AngleBetween(RoverHDG, TargetHDG), speed = RoverSpeed; Delta = Vector3.Distance(mVessel.CoM, TargetPos); DeltaT = Delta / speed; if (Delta > Math.Abs(deg) / 36) { fs.wheelThrottle = mThrottlePID.Control(BrakeSpeed(dc.speed, speed) - speed); if (ForwardAxis != Vector3.zero) { fs.wheelSteer = mWheelPID.Control(deg); } 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); } }
private bool Distance(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { Delta = Math.Abs(dc.target) - Vector3.Distance(RoverOrigPos, mVessel.CoM); DeltaT = Delta / Math.Abs(RoverSpeed); if (Delta > 0) { fs.wheelThrottle = (float)throttlePID.Update(RoverSpeed, dc.speed, -1.0, 1.0); if (mVessel.radarAltitude > minRadarAlt) { Vector3d actuation = pidController.GetActuation(targetRotation); fs.pitch = Mathf.Clamp((float)actuation.x, -driveLimit, driveLimit); fs.roll = Mathf.Clamp((float)actuation.y, -driveLimit, driveLimit); } 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 InitMode(RemoteTech.FlightComputer.Commands.DriveCommand dc) { if (mVessel == null) { RTLog.Verbose("Vessel is null!"); return; } ForwardAxis = Vector3.zero; mRoverAlt = (float)mVessel.altitude; mRoverLat = (float)mVessel.latitude; mRoverLon = (float)mVessel.longitude; Delta = 0; DeltaT = 0; /* Explanation on targetRotation * Quaternion.Euler(x,y,z) - Returns a rotation that rotates z degrees around the z axis, * x degrees around the x axis, and y degrees around the y axis * in that order. * * Unity Q.Euler(0,0,0) isn't matched to KSP's rotation "(0,0,0)" (-90, varying UP-axis, 90) so need to * match the target rotation to the KSP's rotation. * * Rover-specific rotation is Forward (y) to HDG 0, Up (x) to North and * Right (z) to East. */ const float KSPRotXAxis = -90f, KSPRotZAxis = 90f; double AngleFromUpAxis = Mathf.Rad2Deg * Math.Atan(-mVessel.upAxis.z / -mVessel.upAxis.x); float KSPRotYAxis = (float)-AngleFromUpAxis; switch (dc.mode) { case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Turn: mRoverRot = mVessel.ReferenceTransform.rotation; break; case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Distance: targetRotation = Quaternion.Euler(KSPRotXAxis, KSPRotYAxis, KSPRotZAxis); break; case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.DistanceHeading: targetRotation = Quaternion.Euler(KSPRotXAxis - dc.target2, KSPRotYAxis, KSPRotZAxis); break; case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Coord: mTargetLat = dc.target; mTargetLon = dc.target2; targetRotation = Quaternion.Euler(KSPRotXAxis - TargetHDG, KSPRotYAxis, KSPRotZAxis); break; } pidController.setPIDParameters(Kp, Ki, Kd); throttlePID.ResetI(); steerPID.ResetI(); mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false); }
private bool Turn(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { Delta = Math.Abs(Quaternion.Angle(mRoverRot, mVessel.ReferenceTransform.rotation)); DeltaT = Delta / mVessel.angularVelocity.magnitude; if (Delta < dc.target) { fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed); 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 bool Drive(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { if (dc != null) { if (mVessel.srf_velocity.magnitude > 0.5) { float degForward = Mathf.Abs(RTUtil.ClampDegrees90(Vector3.Angle(mVessel.srf_velocity, mVessel.ReferenceTransform.forward))); float degUp = Mathf.Abs(RTUtil.ClampDegrees90(Vector3.Angle(mVessel.srf_velocity, mVessel.ReferenceTransform.up))); float degRight = Mathf.Abs(RTUtil.ClampDegrees90(Vector3.Angle(mVessel.srf_velocity, mVessel.ReferenceTransform.right))); if (degForward < degUp && degForward < degRight) { ForwardAxis = Vector3.forward; } else if (degRight < degUp && degRight < degForward) { ForwardAxis = Vector3.right; } else { ForwardAxis = Vector3.up; } } switch (dc.mode) { case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Turn: return(Turn(dc, fs)); case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Distance: return(Distance(dc, fs)); case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.DistanceHeading: return(DistanceHeading(dc, fs)); case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Coord: return(Coord(dc, fs)); } return(true); } return(true); }
private bool Distance(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { float speed = RoverSpeed; Delta = Math.Abs(dc.target) - Vector3.Distance(RoverOrigPos, mVessel.CoM); DeltaT = Delta / Math.Abs(speed); if (Delta > 0) { fs.wheelThrottle = mThrottlePID.Control(BrakeSpeed(dc.speed, speed) - speed); 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 InitMode(RemoteTech.FlightComputer.Commands.DriveCommand dc) { if (mVessel == null) { MonoBehaviour.print("mVessel was null!!!!!!!!!!!!!!!!!!!!!!!!!!"); } ForwardAxis = Vector3.zero; mRoverAlt = (float)mVessel.altitude; mRoverLat = (float)mVessel.latitude; mRoverLon = (float)mVessel.longitude; Delta = 0; DeltaT = 0; brakeDist = 0; switch (dc.mode) { case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Turn: mRoverRot = mVessel.ReferenceTransform.rotation; break; case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Distance: mWheelPID.setClamp(-1, 1); break; case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.DistanceHeading: mWheelPID.setClamp(-dc.steering, dc.steering); break; case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Coord: mWheelPID.setClamp(-dc.steering, dc.steering); mTargetLat = dc.target; mTargetLon = dc.target2; break; } mThrottlePID.Reset(); mWheelPID.Reset(); mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false); }
private bool DistanceHeading(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs) { float speed = RoverSpeed; Delta = Math.Abs(dc.target) - Vector3.Distance(RoverOrigPos, mVessel.CoM); DeltaT = Delta / speed; if (Delta > 0) { fs.wheelThrottle = mThrottlePID.Control(BrakeSpeed(dc.speed, speed) - speed); if (ForwardAxis != Vector3.zero) { fs.wheelSteer = mWheelPID.Control(RTUtil.AngleBetween(RoverHDG, dc.target2)); } 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); } }