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);
}
}
