private void drawPIDValues(PIDclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (stockPIDDisplay[(int)controllerID])
{
controller.kp = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.kp.ToString(), 45);
controller.ki = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.ki.ToString(), 45);
controller.kd = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.kd.ToString(), 45);
controller.clamp = Math.Max(GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.clamp.ToString(), 45), 0.01);
}
}
private void drawPIDValues(PIDclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.toggleButton);
if (stockPIDDisplay[(int)controllerID])
{
controller.kp = GeneralUI.labPlusNumBox("Kp:", controller.kp.ToString("G3"), 45);
controller.ki = GeneralUI.labPlusNumBox("Ki:", controller.ki.ToString("G3"), 45);
controller.kd = GeneralUI.labPlusNumBox("Kd:", controller.kd.ToString("G3"), 45);
controller.clamp = Math.Max(GeneralUI.labPlusNumBox("Scalar:", controller.clamp.ToString("G3"), 45), 0.01);
}
}
private void drawPIDValues(PIDclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (stockPIDDisplay[(int)controllerID])
{
controller.kp = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.kp.ToString(), 45);
controller.ki = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.ki.ToString(), 45);
controller.kd = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.kd.ToString(), 45);
controller.clamp = Math.Max(GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.clamp.ToString(), 45), 0.01);
}
}
public PIDErrorController(SASList ID, double[] gains)
{
ctrlID = ID;
k_proportional = gains[0];
k_integral = gains[1];
k_derivative = gains[2];
outMin = gains[3];
outMax = gains[4];
integralClampLower = gains[5];
integralClampUpper = gains[6];
scale = gains[7];
easing = gains[8];
}
protected double target_setpoint = 0; // target setpoint
#endregion Fields
#region Constructors
public PIDErrorController(SASList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
{
ctrlID = ID;
k_proportional = Kp;
k_integral = Ki;
k_derivative = Kd;
outMin = OutputMin;
outMax = OutputMax;
integralClampLower = intClampLower;
integralClampUpper = intClampUpper;
scale = scalar;
this.easing = easing;
}
public PIDErrorController(SASList ID, double[] gains)
{
ctrlID = ID;
k_proportional = gains[0];
k_integral = gains[1];
k_derivative = gains[2];
outMin = gains[3];
outMax = gains[4];
integralClampLower = gains[5];
integralClampUpper = gains[6];
scale = gains[7];
easing = gains[8];
}
public PIDErrorController(SASList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
{
ctrlID = ID;
k_proportional = Kp;
k_integral = Ki;
k_derivative = Kd;
outMin = OutputMin;
outMax = OutputMax;
integralClampLower = intClampLower;
integralClampUpper = intClampUpper;
scale = scalar;
this.easing = easing;
}
/// <summary>
/// wait for rate of rotation to fall below 10 degres / s before locking in the target. Derivative only action until that time
/// </summary>
IEnumerator FadeInAxis(SASList axis)
{
updateSetpoint(axis, getCurrentVal(axis));
while (Math.Abs(getCurrentRate(axis, ves) * Mathf.Rad2Deg) > 10)
{
updateSetpoint(axis, getCurrentVal(axis));
yield return(null);
}
orbitalTarget = ves.transform.rotation;
if (axis == SASList.Hdg)
{
currentTarget = getPlaneRotation(flightData.heading, ves);
}
}
private void drawPIDValues(SASList controllerID, string inputName)
{
PID.PID_Controller controller = Utils.GetSAS(controllerID);
controller.bShow = GUILayout.Toggle(controller.bShow, inputName, GeneralUI.toggleButton);
if (controller.bShow)
{
controller.PGain = GeneralUI.labPlusNumBox("Kp:", controller.PGain.ToString("G3"), 45);
controller.IGain = GeneralUI.labPlusNumBox("Ki:", controller.IGain.ToString("G3"), 45);
controller.DGain = GeneralUI.labPlusNumBox("Kd:", controller.DGain.ToString("G3"), 45);
controller.Scalar = GeneralUI.labPlusNumBox("Scalar:", controller.Scalar.ToString("G3"), 45);
fadeReset[(int)controllerID] = Math.Max((float)GeneralUI.labPlusNumBox("Slide:", fadeReset[(int)controllerID].ToString("G3"), 45), 1);
}
}
/// <summary>
/// wait for rate of rotation to fall below 10 degres / s before locking in the target. Derivative only action until that time
/// </summary>
IEnumerator FadeInAxis(SASList axis)
{
updateSetpoint(axis, Utils.getCurrentVal(axis, vesModule.vesselData));
while (Math.Abs(Utils.getCurrentRate(axis, vesModule.vesselRef) * Mathf.Rad2Deg) > 10)
{
updateSetpoint(axis, Utils.getCurrentVal(axis, vesModule.vesselData));
yield return(null);
}
orbitalTarget = vesModule.vesselRef.transform.rotation;
if (axis == SASList.Hdg)
{
currentTarget = Utils.getPlaneRotation(vesModule.vesselData.heading, vesModule);
}
}
private void drawPIDValues(SASList controllerID, string inputName)
{
SASController controller = controllerID.GetSAS(this);
controller.bShow = GUILayout.Toggle(controller.bShow, inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (controller.bShow)
{
controller.PGain = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.PGain.ToString("N3"), 45);
controller.IGain = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.IGain.ToString("N3"), 45);
controller.DGain = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.DGain.ToString("N3"), 45);
controller.Scalar = GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.Scalar.ToString("N3"), 45);
}
}
private void drawPIDValues(SASList controllerID, string inputName)
{
PIDErrorController controller = GetSAS(controllerID);
controller.bShow = GUILayout.Toggle(controller.bShow, inputName, UISkin.customStyles[(int)myStyles.btnToggle]);
if (controller.bShow)
{
controller.PGain = labPlusNumBox(KpLabel, controller.PGain.ToString("N3"), 45);
controller.IGain = labPlusNumBox(KiLabel, controller.IGain.ToString("N3"), 45);
controller.DGain = labPlusNumBox(KdLabel, controller.DGain.ToString("N3"), 45);
controller.Scalar = labPlusNumBox(ScalarLabel, controller.Scalar.ToString("N3"), 45);
}
}
//public static void saveDefaults()
//{
// ConfigNode node = new ConfigNode();
// if (!ReferenceEquals(SSASPreset, null))
// node.AddNode(SSASPresetNode(cP.SSASPreset));
// node.Save(KSPUtil.ApplicationRootPath.Replace("\\", "/") + defaultsPath);
//}
//public static void updateDefaults()
//{
// instance.craftPresetDict[craftDefaultName].AsstPreset.PIDGains = instance.activeAsstPreset.PIDGains;
// instance.craftPresetDict[craftDefaultName].SSASPreset.PIDGains = instance.activeSSASPreset.PIDGains;
// instance.craftPresetDict[craftDefaultName].SASPreset.PIDGains = instance.activeSASPreset.PIDGains;
// instance.craftPresetDict[craftDefaultName].RSASPreset.PIDGains = instance.activeRSASPreset.PIDGains;
// saveDefaults();
//}
public static double[] controllerSASGains(ConfigNode node, SASList type)
{
double[] gains = new double[5];
if (ReferenceEquals(node, null))
return defaultControllerGains(type);
double.TryParse(node.GetValue(pGain), out gains[0]);
double.TryParse(node.GetValue(iGain), out gains[1]);
double.TryParse(node.GetValue(dGain), out gains[2]);
double.TryParse(node.GetValue(scalar), out gains[3]);
double.TryParse(node.GetValue(delay), out gains[4]);
return gains;
}
public static double getCurrentVal(SASList ID)
{
switch (ID)
{
case SASList.Bank:
return(VesselData.Instance.bank);
case SASList.Hdg:
return(VesselData.Instance.heading);
case SASList.Pitch:
default:
return(VesselData.Instance.pitch);
}
}
public static bool hasInput(SASList ID)
{
switch (ID)
{
case SASList.Bank:
return(hasRollInput());
case SASList.Hdg:
return(hasYawInput());
case SASList.Pitch:
default:
return(hasPitchInput());
}
}
public static double getCurrentRate(SASList ID, Vessel v)
{
switch (ID)
{
case SASList.Bank:
return(v.angularVelocity.y);
case SASList.Hdg:
return(v.angularVelocity.z);
case SASList.Pitch:
default:
return(v.angularVelocity.x);
}
}
public static double[] defaultControllerGains(SASList type)
{
switch (type)
{
case SASList.Pitch:
return(Instance.defaultPresetPitchGains);
case SASList.Bank:
return(Instance.defaultPresetRollGains);
case SASList.Hdg:
return(Instance.defaultPresetHdgGains);
default:
return(Instance.defaultPresetPitchGains);
}
}
public static double[] controllerSASGains(ConfigNode node, SASList type)
{
double[] gains = new double[5];
if (node == null)
{
return(defaultControllerGains(type));
}
double.TryParse(node.GetValue(pGain), out gains[0]);
double.TryParse(node.GetValue(iGain), out gains[1]);
double.TryParse(node.GetValue(dGain), out gains[2]);
double.TryParse(node.GetValue(scalar), out gains[3]);
double.TryParse(node.GetValue(slide), out gains[4]);
return(gains);
}
public static double[] defaultControllerGains(SASList type)
{
switch (type)
{
case SASList.Pitch:
return(SurfSAS.defaultPresetPitchGains);
case SASList.Roll:
return(SurfSAS.defaultPresetRollGains);
case SASList.Yaw:
return(SurfSAS.defaultPresetYawGains);
default:
return(SurfSAS.defaultPresetPitchGains);
}
}
private void drawPIDValues(PIDRclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (stockPIDDisplay[(int)controllerID])
{
float kp, ki, kd;
kp = (float)GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.KP.ToString(), 45);
ki = (float)GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.KI.ToString(), 45);
kd = (float)GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.KD.ToString(), 45);
if (kp != controller.KP || ki != controller.KI || kd != controller.KD)
{
controller.ReinitializePIDsOnly(kp, ki, kd);
}
}
}
void setCtrlState(SASList ID, double error, double rate, ref float axisCtrlState)
{
PIDmode mode = PIDmode.PID;
if (!vesModule.vesselRef.checkLanded() && vesModule.vesselRef.IsControllable)
{
mode = PIDmode.PD; // no integral when it can't do anything useful
}
if (allowControl(ID))
{
axisCtrlState = ID.GetSAS(this).ResponseF(error, rate, mode);
}
else if (!Utils.hasInput(ID))
{
axisCtrlState = 0; // kill off stock SAS inputs
}
// nothing happens if player input is present
}
/// <summary>
/// Draws a toggle button and text box of specified widths with update button.
/// </summary>
/// <param name="toggleText"></param>
/// <param name="boxVal"></param>
/// <param name="toggleWidth"></param>
/// <param name="boxWidth"></param>
/// <returns></returns>
public double TogPlusNumBox(string toggleText, SASList controllerID, double currentVal, float toggleWidth, float boxWidth)
{
double setPoint = controllerID.GetSAS(this).SetPoint;
GUILayout.BeginHorizontal();
bool tempState = GUILayout.Toggle(bActive[(int)controllerID], toggleText, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle], GUILayout.Width(toggleWidth));
if (tempState != bActive[(int)controllerID])
{
bActive[(int)controllerID] = tempState;
if (bActive[(int)controllerID])
{
setPoint = currentVal;
targets[(int)controllerID] = currentVal.ToString("0.00");
}
}
string tempText = GUILayout.TextField(targets[(int)controllerID], GeneralUI.UISkin.customStyles[(int)myStyles.numBoxText], GUILayout.Width(boxWidth));
targets[(int)controllerID] = tempText;
if (GUILayout.Button("u"))
{
double temp;
if (double.TryParse(targets[(int)controllerID], out temp))
{
setPoint = temp;
}
bActive[(int)controllerID] = true;
}
GUILayout.EndHorizontal();
return(setPoint);
}
/// <summary>
/// wait for rate of rotation to fall below 10 degres / s before locking in the target. Derivative only action until that time
/// </summary>
IEnumerator FadeInAxis(SASList axis)
{
updateSetpoint(axis, Utils.getCurrentVal(axis, vesModule.vesselData));
while (Math.Abs(Utils.getCurrentRate(axis, vesModule.vesselRef) * Mathf.Rad2Deg) > 10)
{
updateSetpoint(axis, Utils.getCurrentVal(axis, vesModule.vesselData));
yield return null;
}
orbitalTarget = vesModule.vesselRef.transform.rotation;
if (axis == SASList.Hdg)
currentTarget = Utils.getPlaneRotation(vesModule.vesselData.heading, vesModule);
}
private void drawPIDValues(SASList controllerID, string inputName)
{
SASController controller = controllerID.GetSAS(this);
controller.bShow = GUILayout.Toggle(controller.bShow, inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (controller.bShow)
{
controller.PGain = GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.PGain.ToString("N3"), 45);
controller.IGain = GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.IGain.ToString("N3"), 45);
controller.DGain = GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.DGain.ToString("N3"), 45);
controller.Scalar = GeneralUI.labPlusNumBox(GeneralUI.ScalarLabel, controller.Scalar.ToString("N3"), 45);
}
}
public PIDErrorController(SASList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
: base(ID, Kp, Ki, Kd, OutputMin, OutputMax, intClampLower, intClampUpper, scalar, easing)
{
}
public SASController(SASList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
: base(Kp, Ki, Kd, OutputMin, OutputMax, intClampLower, intClampUpper, scalar, easing)
{
ctrlID = ID;
}
public PIDErrorController(SASList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
: base(ID, Kp, Ki, Kd, OutputMin, OutputMax, intClampLower, intClampUpper, scalar, easing)
{
}
public PIDErrorController(SASList ID, double[] gains)
: base(ID, gains)
{
}
void setCtrlState(SASList ID, double error, double rate, ref float axisCtrlState)
{
PIDmode mode = PIDmode.PID;
if (!ves.checkLanded() && ves.IsControllable)
mode = PIDmode.PD; // no integral when it can't do anything useful
if (allowControl(ID))
axisCtrlState = GetSAS(ID).ResponseF(error, rate, mode);
else if (!hasInput(ID))
axisCtrlState = 0; // kill off stock SAS inputs
// nothing happens if player input is present
}
public SASController(SASList ID, double Kp, double Ki, double Kd, double OutputMin, double OutputMax, double intClampLower, double intClampUpper, double scalar = 1, double easing = 1)
: base(Kp, Ki, Kd, OutputMin, OutputMax, intClampLower, intClampUpper, scalar, easing)
{
ctrlID = ID;
}
private void drawPIDValues(SASList controllerID, string inputName)
{
PIDErrorController controller = GetSAS(controllerID);
controller.bShow = GUILayout.Toggle(controller.bShow, inputName, UISkin.customStyles[(int)myStyles.btnToggle]);
if (controller.bShow)
{
controller.PGain = labPlusNumBox(KpLabel, controller.PGain.ToString("N3"), 45);
controller.IGain = labPlusNumBox(KiLabel, controller.IGain.ToString("N3"), 45);
controller.DGain = labPlusNumBox(KdLabel, controller.DGain.ToString("N3"), 45);
controller.Scalar = labPlusNumBox(ScalarLabel, controller.Scalar.ToString("N3"), 45);
}
}
/// <summary>
/// wait for rate of rotation to fall below 10 degres / s before locking in the target. Derivative only action until that time
/// </summary>
IEnumerator FadeInAxis(SASList axis)
{
updateSetpoint(axis, getCurrentVal(axis));
while (Math.Abs(getCurrentRate(axis, ves) * Mathf.Rad2Deg) > 10)
{
updateSetpoint(axis, getCurrentVal(axis));
yield return null;
}
orbitalTarget = ves.transform.rotation;
if (axis == SASList.Hdg)
currentTarget = getPlaneRotation(flightData.heading, ves);
}
bool allowControl(SASList ID)
{
return bActive[(int)ID] && !bPause[(int)ID];
}
/// <summary>
/// Draws a toggle button and text box of specified widths with update button.
/// </summary>
/// <param name="toggleText"></param>
/// <param name="boxVal"></param>
/// <param name="toggleWidth"></param>
/// <param name="boxWidth"></param>
/// <returns></returns>
public double TogPlusNumBox(string toggleText, SASList controllerID, double currentVal, float toggleWidth, float boxWidth)
{
double setPoint = GetSAS(controllerID).SetPoint;
GUILayout.BeginHorizontal();
bool tempState = GUILayout.Toggle(bActive[(int)controllerID], toggleText, UISkin.customStyles[(int)myStyles.btnToggle], GUILayout.Width(toggleWidth));
if (tempState != bActive[(int)controllerID])
{
bActive[(int)controllerID] = tempState;
if (bActive[(int)controllerID])
{
setPoint = currentVal;
targets[(int)controllerID] = currentVal.ToString("0.00");
}
}
string tempText = GUILayout.TextField(targets[(int)controllerID], UISkin.customStyles[(int)myStyles.numBoxText], GUILayout.Width(boxWidth));
targets[(int)controllerID] = tempText;
if (GUILayout.Button("u"))
{
double temp;
if (double.TryParse(targets[(int)controllerID], out temp))
setPoint = temp;
bActive[(int)controllerID] = true;
}
GUILayout.EndHorizontal();
return setPoint;
}
public static double[] defaultControllerGains(SASList type)
{
switch (type)
{
case SASList.Pitch:
return Instance.defaultPresetPitchGains;
case SASList.Bank:
return Instance.defaultPresetRollGains;
case SASList.Hdg:
return Instance.defaultPresetHdgGains;
default:
return Instance.defaultPresetPitchGains;
}
}
public static PID_Controller GetSAS(SASList id)
{
return(SurfSAS.SASControllers[(int)id]);
}
void updateSetpoint(SASList ID, double setpoint)
{
GetSAS(ID).SetPoint = setpoint;
targets[(int)ID] = setpoint.ToString("0.00");
}
public PIDErrorController(SASList ID, double[] gains)
: base(ID, gains)
{
}
public static double getCurrentRate(SASList ID, Vessel v)
{
switch (ID)
{
case SASList.Bank:
return v.angularVelocity.y;
case SASList.Hdg:
return v.angularVelocity.z;
case SASList.Pitch:
default:
return v.angularVelocity.x;
}
}
public SASController(SASList ID, double[] gains)
: base(gains)
{
ctrlID = ID;
}
public static PIDErrorController GetSAS(this SASList id, SurfSAS instance)
{
return(instance.SASControllers[(int)id]);
}
bool allowControl(SASList ID)
{
return(bActive[(int)ID] && !bPause[(int)ID]);
}
public PIDErrorController GetSAS(SASList id)
{
return(SASControllers[(int)id]);
}
void updateSetpoint(SASList ID, double setpoint)
{
ID.GetSAS(this).SetPoint = setpoint;
targets[(int)ID] = setpoint.ToString("0.00");
}
private void drawPIDValues(PIDRclamp controller, string inputName, SASList controllerID)
{
stockPIDDisplay[(int)controllerID] = GUILayout.Toggle(stockPIDDisplay[(int)controllerID], inputName, GeneralUI.UISkin.customStyles[(int)myStyles.btnToggle]);
if (stockPIDDisplay[(int)controllerID])
{
float kp, ki, kd;
kp = (float)GeneralUI.labPlusNumBox(GeneralUI.KpLabel, controller.KP.ToString(), 45);
ki = (float)GeneralUI.labPlusNumBox(GeneralUI.KiLabel, controller.KI.ToString(), 45);
kd = (float)GeneralUI.labPlusNumBox(GeneralUI.KdLabel, controller.KD.ToString(), 45);
if (kp != controller.KP || ki != controller.KI || kd != controller.KD)
controller.ReinitializePIDsOnly(kp, ki, kd);
}
}
public static bool hasInput(SASList ID)
{
switch (ID)
{
case SASList.Bank:
return hasRollInput();
case SASList.Hdg:
return hasYawInput();
case SASList.Pitch:
default:
return hasPitchInput();
}
}
public static double getCurrentVal(SASList ID)
{
switch (ID)
{
case SASList.Bank:
return VesselData.Instance.bank;
case SASList.Hdg:
return VesselData.Instance.heading;
case SASList.Pitch:
default:
return VesselData.Instance.pitch;
}
}
public SASController(SASList ID, double[] gains) : base(gains)
{
ctrlID = ID;
}
public PIDErrorController GetSAS(SASList id)
{
return SASControllers[(int)id];
}
