forked from RemoteTechnologiesGroup/RemoteTech
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathPIDControllerV2.cs
More file actions
78 lines (67 loc) · 2.59 KB
/
Copy pathPIDControllerV2.cs
File metadata and controls
78 lines (67 loc) · 2.59 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
/// <summary>
/// Proportional controller for KSP autopilots
/// </summary>
/// <description>
/// This is a copy of MechJeb's proportional controller, downloaded by Starstrider42
/// from master on June 27, 2014
/// </description>
using System;
namespace RemoteTech.FlightComputer
{
// This PID Controler is used by Raf04 patch for the attitude controler. They have a separate implementation since they use
// a different set of argument and do more (and less) than the other PID controler
public class PIDControllerV2 : IConfigNode
{
public Vector3d intAccum, derivativeAct, propAct;
public double Kp, Ki, Kd, max, min;
public PIDControllerV2(double Kp = 0, double Ki = 0, double Kd = 0, double max = double.MaxValue, double min = double.MinValue)
{
this.Kp = Kp;
this.Ki = Ki;
this.Kd = Kd;
this.max = max;
this.min = min;
Reset();
}
public Vector3d Compute(Vector3d error, Vector3d omega )
{
derivativeAct = omega * Kd;
// integral actíon + Anti Windup
intAccum.x = (Math.Abs(derivativeAct.x) < 0.6 * max) ? intAccum.x + (error.x * Ki * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.x;
intAccum.y = (Math.Abs(derivativeAct.y) < 0.6 * max) ? intAccum.y + (error.y * Ki * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.y;
intAccum.z = (Math.Abs(derivativeAct.z) < 0.6 * max) ? intAccum.z + (error.z * Ki * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.z;
propAct = error * Kp;
Vector3d action = propAct + derivativeAct + intAccum;
// action clamp
action = new Vector3d(Math.Max(min, Math.Min(max, action.x)),
Math.Max(min, Math.Min(max, action.y)),
Math.Max(min, Math.Min(max, action.z)) );
return action;
}
public void Reset()
{
intAccum = Vector3d.zero;
}
public void Load(ConfigNode node)
{
if (node.HasValue("Kp"))
{
Kp = Convert.ToDouble(node.GetValue("Kp"));
}
if (node.HasValue("Ki"))
{
Ki = Convert.ToDouble(node.GetValue("Ki"));
}
if (node.HasValue("Kd"))
{
Kd = Convert.ToDouble(node.GetValue("Kd"));
}
}
public void Save(ConfigNode node)
{
node.SetValue("Kp", Kp.ToString());
node.SetValue("Ki", Ki.ToString());
node.SetValue("Kd", Kd.ToString());
}
}
}