Java 类edu.wpi.first.wpilibj.PIDSource 实例源码

public SetDistanceToBox(double distance) {
    requires(Robot.drivetrain);
    pid = new PIDController(-2, 0, 0, new PIDSource() {
        PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

        public double pidGet() {
            return Robot.drivetrain.getDistanceToObstacle();
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            m_sourceType = pidSource;
        }

        @Override
        public PIDSourceType getPIDSourceType() {
            return m_sourceType;
        }
    }, new PIDOutput() {
        public void pidWrite(double d) {
            Robot.drivetrain.drive(d, d);
        }
    });
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
}

public DriveStraight(double distance) {
    requires(Robot.drivetrain);
    pid = new PIDController(4, 0, 0,
            new PIDSource() {
                PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

                public double pidGet() {
                    return Robot.drivetrain.getDistance();
                }

                @Override
                public void setPIDSourceType(PIDSourceType pidSource) {
                  m_sourceType = pidSource;
                }

                @Override
                public PIDSourceType getPIDSourceType() {
                    return m_sourceType;
                }
            },
            new PIDOutput() { public void pidWrite(double d) {
                Robot.drivetrain.drive(d, d);
            }});
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
}

protected static PIDSource sourceFor(SystemModel model, PIDSourceType initialSourceType) {
    return new PIDSource() {
        private PIDSourceType sourceType = initialSourceType;

        @Override
        public PIDSourceType getPIDSourceType() {
            return sourceType;
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            this.sourceType = pidSource;
        }

        @Override
        public double pidGet() {
            return model.getActualValue();
        }
    };
}

public SwervePod(SpeedController turningMotor, SpeedController driveMotor, Encoder encoder, double encoderDistancePerTick, AngularSensor directionSensor) {
    // Initialize motors //
    _turningMotor = turningMotor;
    _driveMotor = driveMotor;

    // Initialize sensors //
    _encoder = encoder;
    _encoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate);
    _encoder.setDistancePerPulse(encoderDistancePerTick);
    _encoder.start();
    _directionSensor = directionSensor;

    // Initialize PID loops //
    // Turning //
    PIDTurning = new PIDController(Kp_turning, Ki_turning, Kd_turning, _directionSensor, _turningMotor);
    PIDTurning.setOutputRange(minSpeed, maxSpeed);
    PIDTurning.setContinuous(true);
    PIDTurning.setAbsoluteTolerance(tolerance_turning);
    PIDTurning.enable();

    // Linear driving //
    PIDDriving = new PIDController(Kp_driving, Ki_driving, Kd_driving, _encoder, _driveMotor);
    PIDDriving.setOutputRange(minSpeed, maxSpeed);
    PIDDriving.disable(); //TODO: Enable
}

/**
 * Constructs an EasyPID object with the given parameters
 * @param p the constant P value
 * @param i the constant I value
 * @param d the constant D value
 * @param f the constant F value
 * @param name the name to be given to the EasyPID object for SmartDashboard
 * @param s the source to be used for input in the PIDController object
 */
public EasyPID(double p, double i, double d, double f, String name, PIDSource s)
{
    this.name = name;
    System.out.println("constucting PIDEasy object");
    source = s;
    output = new SoftPID();

    P = p;
    I = i;
    D = d;
    F = f;

    controller = new PIDController(P, I, D, F, source, output);
    SmartDashboard.putData(name, controller);
}

public DriveToPeg(){        
    double distance = .2;

    requires(Robot.driveBase);
    double kP = -.4;
    double kI = 1;
    double kD = 5;

    pid = new PIDController(kP, kI, kD, new PIDSource() {
        PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

        @Override
        public double pidGet() {
            return Robot.driveBase.getDistanceAhead();
        }

        @Override
        public void setPIDSourceType(PIDSourceType pidSource) {
            m_sourceType = pidSource;
        }

        @Override
        public PIDSourceType getPIDSourceType() {
            return m_sourceType;
        }
    }, new PIDOutput() {
        @Override
        public void pidWrite(double d) {
            Robot.driveBase.driveForward(d);
            System.out.println(d);
        }
    });
    pid.setAbsoluteTolerance(0.01);
    pid.setSetpoint(distance);
    pid.setOutputRange(0, .35);

    SmartDashboard.putData("driveToPeg", pid);
}

public NavXGyroWrapper(PIDSource wrappedSource) throws IllegalSourceException {
    super(wrappedSource);
    if (wrappedSource.getClass() != AHRS.class) {
        throw new IllegalSourceException();
    }
    else {
        m_navxSource = ((AHRS) m_pidSource);
    }
}

public NavXAccelWrapper(PIDSource wrappedSource, Axis axis) throws IllegalSourceException {
    super(wrappedSource);
    if (wrappedSource.getClass() != AHRS.class) {
        throw new IllegalSourceException();
    }
    else {
        m_navxSource = ((AHRS) m_pidSource);
        this.axis = axis;
    }
}

public DriveToAngle(double angle, PIDSource source) {
    requires(Robot.driveTrain);
    setPoint = angle;

    Robot.driveTrain.ahrs.reset();

    controller = new PIDController(0.75, 0, 0.9, 0, source, this);
    controller.setInputRange(-180, 180);
    controller.setOutputRange(-.26, .26);
    controller.setAbsoluteTolerance(1.0);
    controller.setContinuous(true);

    LiveWindow.addActuator("DriveToAngle", "RotationController", controller);
}

public DriveToDistance(double distance, PIDSource source) {
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
    requires(Robot.driveTrain);
    setPoint = distance;

    controller = new PIDController(0.3, 0, 1, source, this);
    controller.setOutputRange(-.28, .28);
    controller.setAbsoluteTolerance(.5);
}

/**
 * Create a linear FIR or IIR filter.
 *
 * @param source  The PIDSource object that is used to get values
 * @param ffGains The "feed forward" or FIR gains
 * @param fbGains The "feed back" or IIR gains
 */
public LinearDigitalFilter(PIDSource source, double[] ffGains,
                           double[] fbGains) {
  super(source);
  m_inputs = new CircularBuffer(ffGains.length);
  m_outputs = new CircularBuffer(fbGains.length);
  m_inputGains = ffGains;
  m_outputGains = fbGains;
}

/**
 * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
 * x[0]).
 *
 * <p>This filter is always stable.
 *
 * @param source The PIDSource object that is used to get values
 * @param taps   The number of samples to average over. Higher = smoother but slower
 * @throws IllegalArgumentException if number of taps is less than 1
 */
public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
  if (taps <= 0) {
    throw new IllegalArgumentException("Number of taps was not at least 1");
  }

  double[] ffGains = new double[taps];
  for (int i = 0; i < ffGains.length; i++) {
    ffGains[i] = 1.0 / taps;
  }

  double[] fbGains = new double[0];

  return new LinearDigitalFilter(source, ffGains, fbGains);
}

public void setPIDSource(PIDSource source)
{
    turnController = new PIDController(kP, kI, kD, kF, source, this);
    turnController.setInputRange(-15.0, 15.0);
    turnController.setOutputRange(-1.0, 1.0);
    turnController.setAbsoluteTolerance(kToleranceDegrees);
    turnController.setContinuous(false);
}

public PIDSource getSource(final int index) {
    return new PIDSource() {
        public double pidGet() {
            return vals[index];
        }
    };
}

public ChassisSubsystem() {
    encLeft.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate);
    encRight.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate);

    encLeft.start();
    encRight.start();

    pidLeft.setInputRange(-Constants.MAX_ENCODER_COUNTS, Constants.MAX_ENCODER_COUNTS);
    pidRight.setInputRange(-Constants.MAX_ENCODER_COUNTS, Constants.MAX_ENCODER_COUNTS);

    pidLeft.setOutputRange(-1.0, 1.0);
    pidRight.setOutputRange(-1.0, 1.0);
}

public BangBangController (PIDOutput pidOutput, PIDSource pidSource, double period){
    _pidSource = pidSource;
    _pidOutput = pidOutput;
    _period = period;
    _setPoint = 0;
    _timer.schedule(new _bgTask(), 0, (long) (1000*_period));
}

public TakeBackHalfPlusPlus(PIDOutput pidoutput, PIDSource pidsource, double period, double max, double min){

    _pidSource = pidsource;
    _pidOutput = pidoutput;
    _setPoint = 0;
    _period = period;
    _max = max;
    _min = min;
    _timer.schedule(new _bgTask(), 0, (long) (1000 * _period));
}

/**
 * Allocate a PID object with the given constants for P, I, D, and F
 *
 * @param Kp the proportional coefficient
 * @param Ki the integral coefficient
 * @param Kd the derivative coefficient
 * @param Kf the feed forward term
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output percentage
 * @param period the loop time for doing calculations. This particularly
 * effects calculations of the integral and differential terms. The default
 * is 50ms.
 */
public PIDController649(double Kp, double Ki, double Kd, double Kf,
        PIDSource source, PIDOutput output,
        double period) {

    if (source == null) {
        throw new NullPointerException("Null PIDSource was given");
    }
    if (output == null) {
        throw new NullPointerException("Null PIDOutput was given");
    }

    m_controlLoop = new java.util.Timer();

    m_P = Kp;
    m_I = Ki;
    m_D = Kd;
    m_F = Kf;

    m_pidInput = source;
    m_pidOutput = output;
    m_period = period;

    m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000));

    instances++;
    UsageReporting.report(UsageReporting.kResourceType_PIDController, instances);
    m_tolerance = new NullTolerance();
    m_outputDirection = true;
}

public VariablePIDInput(PIDSource wrappedSource) {
    m_pidSource = wrappedSource;
}

public PIDSpeedController(PIDSource source, PIDOutput output, String subsystem, String controllerName) {
    controller = new PIDController(0, 0, 0, 0, source, output);

    LiveWindow.addActuator(subsystem, controllerName, controller);
}

public Filter(PIDSource source) {
  m_source = source;
}

public PIDSpeedController(PIDSource source, PIDOutput output, String subsystem, String controllerName) {
    controller = new PIDController(0, 0, 0, 0, source, output);

    LiveWindow.addActuator(subsystem, controllerName, controller);
}

protected static PIDSource sourceFor(SystemModel model) {
    return sourceFor(model, PIDSourceType.kRate);
}

/**
 * Read the input, calculate the output accordingly, and write to the
 * output. This should only be called by the PIDTask and is created during
 * initialization.
 */
private void calculate() {
    boolean enabled;
    PIDSource pidInput;

    synchronized (this) {
        if (m_pidInput == null) {
            return;
        }
        if (m_pidOutput == null) {
            return;
        }
        enabled = m_enabled; // take snapshot of these values...
        pidInput = m_pidInput;
    }

    if (enabled) {
        double input;
        input = pidInput.pidGet();

        double result;
        PIDOutput pidOutput = null;

        synchronized (this) {
            m_error = m_setpoint - input;
            if (m_continuous) {
                if (Math.abs(m_error)
                        > (m_maximumInput - m_minimumInput) / 2) {
                    if (m_error > 0) {
                        m_error = m_error - m_maximumInput + m_minimumInput;
                    } else {
                        m_error = m_error
                                + m_maximumInput - m_minimumInput;
                    }
                }
            }

            if (m_I != 0) {
                double potentialIGain = (m_totalError + m_error) * m_I;
                if (potentialIGain < m_maximumOutput) {
                    if (potentialIGain > m_minimumOutput) {
                        m_totalError += m_error;
                    } else {
                        m_totalError = m_minimumOutput / m_I;
                    }
                } else {
                    m_totalError = m_maximumOutput / m_I;
                }
            }
            //******************************************************************************************************************
            if (m_error > 0 && m_prevError < 0 || m_error < 0 && m_prevError > 0) {
                m_totalError = 0;
            }
            //******************************************************************************************************************
            m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F;
            m_errorDiff = m_prevError - m_error;
            m_prevError = m_error;

            if (m_result > m_maximumOutput) {
                m_result = m_maximumOutput;
            } else if (m_result < m_minimumOutput) {
                m_result = m_minimumOutput;
            }
            pidOutput = m_pidOutput;
            result = m_result * (m_outputDirection ? 1 : -1);
        }
        pidOutput.pidWrite(result);
    }
}

public static void init() {

    //initialize encoders
    frontWheelEncoder = new Encoder(1, 1, 1, 2, false, CounterBase.EncodingType.k2X);
    frontWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK);
    frontWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    frontWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "frontWheelEncoder", frontWheelEncoder);
    leftWheelEncoder = new Encoder(1, 3, 1, 4, false, CounterBase.EncodingType.k2X);
    leftWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK); 
    leftWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    leftWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "leftWheelEncoder", leftWheelEncoder);
    backWheelEncoder = new Encoder(1, 5, 1, 6, false, CounterBase.EncodingType.k2X);
    backWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK); 
    backWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    backWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "backWheelEncoder", backWheelEncoder); 
    rightWheelEncoder = new Encoder(1, 7, 1, 8, false, CounterBase.EncodingType.k2X);
    rightWheelEncoder.setDistancePerPulse(WHEEL_ENCODER_DISTANCE_PER_TICK); 
    rightWheelEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kRate); // have encoder measure rate, not distance
    rightWheelEncoder.start();
    LiveWindow.addSensor("Drivetrain", "rightWheelEncoder", rightWheelEncoder); 

    frontModuleEncoder = new Encoder(2, 1, 2, 2, false, CounterBase.EncodingType.k2X);
    frontModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    frontModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    frontModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "frontModuleEncoder", frontModuleEncoder);
    leftModuleEncoder = new Encoder(2, 7, 2, 8, false, CounterBase.EncodingType.k2X);
    leftModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    leftModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    leftModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "leftModuleEncoder", leftModuleEncoder);
    backModuleEncoder = new Encoder(2, 5, 2, 6, false, CounterBase.EncodingType.k2X);
    backModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    backModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    backModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "backModuleEncoder", backModuleEncoder);
    rightModuleEncoder = new Encoder(2, 3, 2, 4, false, CounterBase.EncodingType.k2X);
    rightModuleEncoder.setDistancePerPulse(MODULE_ENCODER_DISTANCE_PER_TICK); 
    rightModuleEncoder.setPIDSourceParameter(PIDSource.PIDSourceParameter.kDistance); // have encoder measure rate, not distance
    rightModuleEncoder.start();
    LiveWindow.addSensor("Drivetrain", "rightModuleEncoder", rightModuleEncoder);

    //initialize motors
    frontWheelMotor = new Victor246(1,1);
    LiveWindow.addActuator("Drivetrain", "frontWheelMotor", (Victor) frontWheelMotor);
    leftWheelMotor = new Victor246(1,2);
    LiveWindow.addActuator("Drivetrain", "leftWheelMotor", (Victor) leftWheelMotor);
    backWheelMotor = new Victor246(1,3);
    LiveWindow.addActuator("Drivetrain", "backWheelMotor", (Victor) backWheelMotor);
    rightWheelMotor = new Victor246(1,4);
    LiveWindow.addActuator("Drivetrain", "rightWheelMotor", (Victor) rightWheelMotor);

    frontModuleMotor = new Jaguar246(2,1);
    LiveWindow.addActuator("Drivetrain", "frontModuleMotor", (Jaguar) frontModuleMotor);
    leftModuleMotor = new Jaguar246(2,2);
    LiveWindow.addActuator("Drivetrain", "leftModuleMotor", (Jaguar) leftModuleMotor);
    backModuleMotor = new Jaguar246(2,3);
    LiveWindow.addActuator("Drivetrain", "backModuleMotor", (Jaguar) backModuleMotor);
    rightModuleMotor = new Jaguar246(2,4);
    LiveWindow.addActuator("Drivetrain", "rightModuleMotor", (Jaguar) rightModuleMotor);

    //initialize limit switch
    angleZeroingButton = new DigitalInput(1,9);
    LiveWindow.addSensor("Drivetrain", "encoderZeroingSwitch", angleZeroingButton);
}

public EasyPID(String name, PIDSource s)
{
    this(0.0, 0.0, 0.0, 0.0, name, s);
}

/**
 * Constructs an EasyPID object with the given source
 * @param s the source that should be used for the PIDController input
 */
public EasyPID(PIDSource s)
{
    this("EasyPID", s);
}

/**
 * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
 * gain = e^(-dt / T), T is the time constant in seconds.
 *
 * <p>This filter is stable for time constants greater than zero.
 *
 * @param source       The PIDSource object that is used to get values
 * @param timeConstant The discrete-time time constant in seconds
 * @param period       The period in seconds between samples taken by the user
 */
public static LinearDigitalFilter singlePoleIIR(PIDSource source,
                                                double timeConstant,
                                                double period) {
  double gain = Math.exp(-period / timeConstant);
  double[] ffGains = {1.0 - gain};
  double[] fbGains = {-gain};

  return new LinearDigitalFilter(source, ffGains, fbGains);
}

/**
 * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
 * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
 *
 * <p>This filter is stable for time constants greater than zero.
 *
 * @param source       The PIDSource object that is used to get values
 * @param timeConstant The discrete-time time constant in seconds
 * @param period       The period in seconds between samples taken by the user
 */
public static LinearDigitalFilter highPass(PIDSource source,
                                           double timeConstant,
                                           double period) {
  double gain = Math.exp(-period / timeConstant);
  double[] ffGains = {gain, -gain};
  double[] fbGains = {-gain};

  return new LinearDigitalFilter(source, ffGains, fbGains);
}

/**
 * Creates a {@link PIDController} from a {@link PIDConstants} object and
 * the specified source and output.
 * 
 * @param constants the PID constants
 * @param source the PID source
 * @param output the PID output
 * @return a shiny new PID controller
 */
public static PIDController createPIDControllerFromConstants(PIDConstants constants,
        PIDSource source, PIDOutput output) {
    return new PIDController(constants.p, constants.i, constants.d, constants.f, source, output);
}

/**
 * Allocate a PID object with the given constants for P, I, D and period
 *
 * @param Kp
 * @param Ki
 * @param Kd
 * @param source
 * @param output
 * @param period
 */
public PIDController649(double Kp, double Ki, double Kd,
        PIDSource source, PIDOutput output,
        double period) {
    this(Kp, Ki, Kd, 0.0, source, output, period);
}

/**
 * Allocate a PID object with the given constants for P, I, D, using a 50ms
 * period.
 *
 * @param Kp the proportional coefficient
 * @param Ki the integral coefficient
 * @param Kd the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output percentage
 */
public PIDController649(double Kp, double Ki, double Kd,
        PIDSource source, PIDOutput output) {
    this(Kp, Ki, Kd, source, output, kDefaultPeriod);
}

/**
 * Allocate a PID object with the given constants for P, I, D, using a 50ms
 * period.
 *
 * @param Kp the proportional coefficient
 * @param Ki the integral coefficient
 * @param Kd the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output percentage
 */
public PIDController649(double Kp, double Ki, double Kd, double Kf,
        PIDSource source, PIDOutput output) {
    this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
}