// ziegler nichols method below for PID:
// private readonly double[] pidConstants = new double[3] { 0.6* Ku, 0.6 * 2 * Ku / Pu, 0.6 * Ku * Pu / 8.0 };
// original calculated values above... private readonly double[] pidConstants = new double[3] { 0.711, 1.422, .088875 };
// private readonly double[] pidConstants = new double[3] { 0.711, 1.422, .088875 };
//private readonly double[] pidConstants = new double[3] { 0.40, 1.10, .088875 };
//private readonly double[] pidConstants = new double[3] { 0.6, 1, 0 };
public ControlledHolonomicDrive(IAngleFinder compass, params Wheel[] wheels)
: base(wheels)
{
_compass = compass;
_pid = new PIDController(pidConstants)
{
Input = new PIDController.InputFunction(
() => _compass.Angle,
Range.Angle
),
Output = new PIDController.OutputFunction(
(value) => base.TurnVelocity = -value,
Range.SignedAngle * 2
),
Continuous = true,
SetPoint = 0,
ErrorLimit = new Range(MathEx.TwoPi / 8)
};
}
public FieldHeadingFinder(IAngleFinder angleFinder)
{
AngleFinder = angleFinder;
RecordNorthDirection();
}
