public void InitMode(DriveCommand dc)
{
UpdateAxis();
mRoverAlt = (float)mVessel.altitude;
mRoverLat = (float)mVessel.latitude;
mRoverLon = (float)mVessel.longitude;
switch (dc.mode)
{
case DriveCommand.DriveMode.Turn:
mRoverRot = mVessel.ReferenceTransform.rotation;
break;
case DriveCommand.DriveMode.Distance:
mWheelPID.setClamp(-1, 1);
mKeepHDG = RoverHDG;
break;
case DriveCommand.DriveMode.DistanceHeading:
mWheelPID.setClamp(-dc.steering, dc.steering);
break;
case 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 void HDG()
{
if (Event.current.Equals(Event.KeyboardEvent("return")) && GUI.GetNameOfFocusedControl().StartsWith("RC"))
{
mFlightComputer.Enqueue(DriveCommand.DistanceHeading(Dist, Heading, mSteerClamp, Speed));
}
GUILayout.BeginHorizontal();
{
GUILayout.Label("Wheel: ");
GUILayout.FlexibleSpace();
GUILayout.Label(mSteerClamp.ToString("P"));
GUILayout.Label("Clamp", GUILayout.Width(40));
}
GUILayout.EndHorizontal();
RTUtil.HorizontalSlider(ref mSteerClamp, 0, 1);
GUILayout.BeginHorizontal();
{
GUILayout.Label("Hdg.", GUILayout.Width(50));
GUI.SetNextControlName("RC1");
RTUtil.TextField(ref Mheading, GUILayout.Width(50), GUILayout.ExpandWidth(false));
GUILayout.Label("(°)", GUI.skin.textField, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
{
GUILayout.Label("Dist.", GUILayout.Width(50));
GUI.SetNextControlName("RC2");
RTUtil.TextField(ref mDist, GUILayout.Width(50), GUILayout.ExpandWidth(false));
GUILayout.Label("(m)", GUI.skin.textField, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
{
GUILayout.Label("Speed", GUILayout.Width(50));
GUI.SetNextControlName("RC3");
RTUtil.TextField(ref mSpeed, GUILayout.Width(50), GUILayout.ExpandWidth(false));
GUILayout.Label("(m/s)", GUI.skin.textField, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
Mheading = RTUtil.ConstrictNum(Mheading, 360);
mDist = RTUtil.ConstrictNum(mDist, false);
mSpeed = RTUtil.ConstrictNum(mSpeed, false);
}
private void DistanceHeading(DriveCommand dc, FlightCtrlState fs)
{
if (Vector3.Distance(RoverOrigPos, mVessel.CoM) < Math.Abs(dc.target))
{
fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed);
fs.wheelSteer = mWheelPID.Control(RTUtil.ClampDegrees180(RoverHDG - dc.target2));
}
else
{
fs.wheelThrottle = 0;
fs.wheelSteer = 0;
mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
dc.mode = DriveCommand.DriveMode.Off;
dc = null;
}
}
private void Turn(DriveCommand dc, FlightCtrlState fs)
{
ModuleWheel m = new ModuleWheel();
if (Math.Abs(Quaternion.Angle(mRoverRot, mVessel.ReferenceTransform.rotation)) < dc.target)
{
fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed);
fs.wheelSteer = dc.steering;
}
else
{
fs.wheelThrottle = 0;
fs.wheelSteer = 0;
mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
dc.mode = DriveCommand.DriveMode.Off;
dc = null;
}
}
public void InitMode(DriveCommand dc)
{
axis = ModuleWheel.AlignmentAxis.None;
foreach (Part p in mVessel.parts)
{
if (p.Modules.Contains("ModuleWheel"))
{
axis = (p.Modules["ModuleWheel"] as ModuleWheel).alignmentAxis;
if (axis != ModuleWheel.AlignmentAxis.None)
{
break;
}
}
}
mRoverAlt = (float)mVessel.altitude;
mRoverLat = (float)mVessel.latitude;
mRoverLon = (float)mVessel.longitude;
switch (dc.mode)
{
case DriveCommand.DriveMode.Turn:
mRoverRot = mVessel.ReferenceTransform.rotation;
break;
case DriveCommand.DriveMode.Distance:
mWheelPID.setClamp(-1, 1);
mKeepHDG = RoverHDG;
break;
case DriveCommand.DriveMode.DistanceHeading:
mWheelPID.setClamp(-dc.steering, dc.steering);
break;
case 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 void Coord(DriveCommand dc, FlightCtrlState fs)
{
float
dist = Vector3.Distance(mVessel.CoM, TargetPos),
deg = RTUtil.ClampDegrees180(RoverHDG - TargetHDG);
if (dist > Math.Abs(deg) / 36)
{
fs.wheelThrottle = mThrottlePID.Control(dc.speed - RoverSpeed);
fs.wheelSteer = mWheelPID.Control(deg);
}
else
{
fs.wheelThrottle = 0;
fs.wheelSteer = 0;
mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
dc.mode = DriveCommand.DriveMode.Off;
dc = null;
}
}
public void Drive(DriveCommand dc, FlightCtrlState fs)
{
if (dc != null)
{
switch (dc.mode)
{
case DriveCommand.DriveMode.Turn:
Turn(dc, fs);
break;
case DriveCommand.DriveMode.Distance:
Distance(dc, fs);
break;
case DriveCommand.DriveMode.DistanceHeading:
DistanceHeading(dc, fs);
break;
case DriveCommand.DriveMode.Coord:
Coord(dc, fs);
break;
}
}
}
private void Target()
{
if (Event.current.Equals(Event.KeyboardEvent("return")) && GUI.GetNameOfFocusedControl().StartsWith("RC"))
{
mFlightComputer.Enqueue(DriveCommand.Coord(mSteerClamp, Latitude, Longitude, Speed));
}
else if (GameSettings.MODIFIER_KEY.GetKey() && ((Input.GetMouseButton(0) || Input.GetMouseButton(1)) != MouseClick))
{
MouseClick = Input.GetMouseButton(0) || Input.GetMouseButton(1);
Vector2 latlon;
if (MouseClick && RTUtil.CBhit(mFlightComputer.mAttachedVessel.mainBody, out latlon))
{
Latitude = latlon.x;
Longitude = latlon.y;
if (Input.GetMouseButton(1))
{
mFlightComputer.Enqueue(DriveCommand.Coord(mSteerClamp, Latitude, Longitude, Speed));
}
}
}
GUILayout.BeginHorizontal();
{
GUILayout.Label("Wheel: ");
GUILayout.FlexibleSpace();
GUILayout.Label(mSteerClamp.ToString("P"));
GUILayout.Label("Clamp", GUILayout.Width(40));
}
GUILayout.EndHorizontal();
RTUtil.HorizontalSlider(ref mSteerClamp, 0, 1);
GUILayout.BeginHorizontal();
{
GUILayout.Label("LAT.", GUILayout.Width(50));
GUI.SetNextControlName("RC1");
RTUtil.TextField(ref mLatitude, GUILayout.Width(50), GUILayout.ExpandWidth(false));
GUILayout.Label("(°)", GUI.skin.textField, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
{
GUILayout.Label("LON.", GUILayout.Width(50));
GUI.SetNextControlName("RC2");
RTUtil.TextField(ref mLongditude, GUILayout.Width(50), GUILayout.ExpandWidth(false));
GUILayout.Label("(°)", GUI.skin.textField, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
{
GUILayout.Label("Speed", GUILayout.Width(50));
GUI.SetNextControlName("RC3");
RTUtil.TextField(ref mSpeed, GUILayout.Width(50), GUILayout.ExpandWidth(false));
GUILayout.Label("(m/s)", GUI.skin.textField, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
mLatitude = RTUtil.ConstrictNum(mLatitude);
mLongditude = RTUtil.ConstrictNum(mLongditude);
mSpeed = RTUtil.ConstrictNum(mSpeed, false);
}
private void EnqueueDist()
{
mFlightComputer.Enqueue(DriveCommand.Distance(Dist, 0, Speed));
}
private void EnqueueTurn()
{
mFlightComputer.Enqueue(DriveCommand.Turn(mSteering, Turn, Speed));
}
