コード例 #1
0
 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)
     };
 }