public Goalie()
{
backUS = robot.Sensors.US[2];
positionPid = new PIDController(0.5)
{
Input = new PIDController.InputFunction(
() => backUS.DistanceCM,
Range.Angle
),
Output = new PIDController.OutputFunction(
(value) => robot.Drive.DriveVelocity = new Vector(value, robot.Drive.DriveVelocity.X),
Range.SignedAngle * 2
),
Continuous = true,
SetPoint = 20
};
}
// 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)
};
}