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

/**
 * Requires DriveTrain, DriveEncoders, Gyro, Ultrasonic
 * @param setpoint distance to move in inches
 */
public DriveDistanceDiff(double setpoint){
    requires(Robot.drivetrain);
    requires(Robot.driveEncoders);
    requires(Robot.ultrasonic);
    this.setpoint=setpoint;
    pid = new PIDController(0.1, 0, 0, Robot.driveEncoders.getEncoderLeft(), new pidOutput());
    pidRight = new PIDController(0.1,0,0,Robot.driveEncoders.getEncoderRight(),new pidOutputRight());
    if(setpoint>0){
        pid.setInputRange(0,setpoint+10);
        pidRight.setInputRange(0,setpoint+10);
    }else{
        pid.setInputRange(setpoint-10,0);
        pidRight.setInputRange(setpoint-10,0);
    }
    pid.setAbsoluteTolerance(0.5);
    pidRight.setAbsoluteTolerance(0.5);
    pid.setOutputRange(-0.6,0.6);
    pidRight.setOutputRange(-0.6,0.6);
    pid.setSetpoint(setpoint);
    pidRight.setSetpoint(setpoint);
}

/**
 * Requires DriveTrain, DriveEncoders, Gyro, Ultrasonic
 * @param setpoint distance to move in inches
 */
public DriveDistance(double setpoint, boolean stoppable, double turn){
    requires(Robot.drivetrain);
    requires(Robot.driveEncoders);
    requires(Robot.gyro);
    requires(Robot.ultrasonic);
    this.setpoint=setpoint;
    this.stoppable=stoppable;
    this.turn=turn;
    pid = new PIDController(0.40, 0, 0, Robot.driveEncoders.getEncoderLeft(), new pidOutput());
    pidTurn = new PIDController(0.2, 0.02, 0.4, Robot.gyro.getGyro(), new pidOutputTurn());

    pidTurn.setOutputRange(-0.50,0.50);
    if(setpoint>0){
        pid.setInputRange(0,setpoint+10);
    }else{
        pid.setInputRange(setpoint-10,0);
    }
    pid.setAbsoluteTolerance(0.5);
    pid.setOutputRange(-0.6,0.6);
    pid.setSetpoint(setpoint);
    System.out.println("Driving Forward for:  " + setpoint);
}

/**
 * Creates a JsonAutonomous from the specified file
 * @param file The location of the file to parse
 */
public JsonAutonomous(String file)
{
    ap_ds = DriverStation.getInstance();
    turn = new PIDController(0.02, 0, 0, Robot.navx, this);
    turn.setInputRange(-180, 180);
    turn.setOutputRange(-0.7, 0.7);
    turn.setAbsoluteTolerance(0.5);
    turn.setContinuous(true);

    straighten = new PIDController(0.01, 0, 0, Robot.navx, this);
    straighten.setInputRange(-180, 180);
    straighten.setOutputRange(-0.7, 0.7);
    straighten.setAbsoluteTolerance(0);
    straighten.setContinuous(true);

    turn.setPID(Config.getSetting("auto_turn_p", 0.02), 
            Config.getSetting("auto_turn_i", 0.00001),
            Config.getSetting("auto_turn_d", 0));
    straighten.setPID(Config.getSetting("auto_straight_p", 0.2), 
            Config.getSetting("auto_straight_i", 0),
            Config.getSetting("auto_straight_d", 0));
    parseFile(file);
}

/**
 * Constructor specifying setpoint angle.
 * @param setPointAngle
 */
public AutoTurnToPeg(double setPointAngle) {
    requires(Robot.driveTrain);
    //requires(Robot.ultrasonics);
    //requires(Robot.visionTest);
    requires(Robot.navx);

    this.setPointAngle = setPointAngle;

    turnController = new PIDController(kP, kI, kD, Robot.navx.getAHRS(), new MyPidOutput());
    turnController.setInputRange(-180, 180);
    turnController.setOutputRange(-maxOutputRange, maxOutputRange);
    turnController.setAbsoluteTolerance(kToleranceDegrees);
    turnController.setContinuous(true);

    /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
    /* tuning of the Turn Controller's P, I and D coefficients.            */
    /* Typically, only the P value needs to be modified.                   */
    //LiveWindow.addSensor("DriveSystem", "RotateController", turnController);
}

/**
* 
* @param distance target distance in inches
* @param timeOut time out in milliseconds
*/
  public AutoDriveDistance(double distance, long timeOut) {
      requires(Robot.driveTrain);
      //requires(Robot.ultrasonics);
      requires(Robot.navx);

      targetDistance = distance;
      timeMax = timeOut;

      turnController = new PIDController(kP, kI, kD, Robot.driveTrain, new MyPidOutput());
      //turnController.setInputRange(-180, 180);
      turnController.setOutputRange(-maxOutputRange, maxOutputRange);
      turnController.setAbsoluteTolerance(kToleranceDistance);
      turnController.setContinuous(true);

      /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
      /* tuning of the Turn Controller's P, I and D coefficients.            */
      /* Typically, only the P value needs to be modified.                   */
      LiveWindow.addSensor("DriveSystem", "RotateController", turnController);
  }

private AutoSteerDriveToPeg() {
    requires(Robot.driveTrain);
    //requires(Robot.ultrasonics);
    //requires(Robot.visionTest);
    requires(Robot.navx);

    turnController = new PIDController(kP, kI, kD, Robot.navx.getAHRS(), new MyPidOutput());
    turnController.setInputRange(-maxAbsSetpoint, maxAbsSetpoint);
    turnController.setOutputRange(-maxOutputRange, maxOutputRange);
    turnController.setAbsoluteTolerance(kToleranceDegrees);
    turnController.setContinuous(true);

    /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
    /* tuning of the Turn Controller's P, I and D coefficients.            */
    /* Typically, only the P value needs to be modified.                   */
    LiveWindow.addSensor("DriveSystem", "RotateController", turnController);
}

/**
* 
* @param speed forward speed is positive volts
*/
  public AutoTurnByVision(double speed) {
      requires(Robot.driveTrain);
      //requires(Robot.ultrasonics);
      requires(Robot.visionTest);
      requires(Robot.navx);

      forwardSpeed = -speed;
      turnController = new PIDController(kP, kI, kD, Robot.navx.getAHRS()/*Robot.visionTest*/, new MyPidOutput());
      turnController.setInputRange(-180, 180);
      turnController.setOutputRange(-maxOutputRange, maxOutputRange);
      turnController.setAbsoluteTolerance(kToleranceDegrees);
      turnController.setContinuous(true); // TODO is this what we want???

      /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
      /* tuning of the Turn Controller's P, I and D coefficients.            */
      /* Typically, only the P value needs to be modified.                   */
      LiveWindow.addSensor("DriveSystem", "RotateController", turnController);
  }

public DriveTrain() {
    // TODO: would be nice to migrate stuff from RobotMap here.

    // m_turnPID is used to improve accuracy during auto-turn operations.
    m_imu = new IMUPIDSource();
    m_turnPID = new PIDController(turnKp, turnKi, turnKd, turnKf,
                                  m_imu,
                                  new PIDOutput() {
                public void pidWrite(double output) {
                    // output is [-1, 1]... we need to
                    // convert this to a speed...
                    Robot.driveTrain.turn(output * MAXIMUM_TURN_SPEED);
                    // robotDrive.tankDrive(-output, output);
                }
            });
    m_turnPID.setOutputRange(-1, 1);
    m_turnPID.setInputRange(-180, 180);
    m_turnPID.setPercentTolerance(2);
    // m_turnPID.setContuous?
}

public Drivetrain() {   
    leftWheelsPIDSource = new EncoderPIDSource(RobotMap.driveEncoderLeft, 1.0 / 14.0, PIDSourceType.kDisplacement);
    //rightWheelsPIDSource = new EncoderPIDSource(RobotMap.driveEncoderRight, -1.0 / 14.0, PIDSourceType.kDisplacement);
    rightWheelsPIDSource = new EncoderPIDSource(RobotMap.driveEncoderRight, ((-1.0 / 360.0) * 250.0) * (1.0 / 14.0), PIDSourceType.kDisplacement);

    compassPID = new PIDController(0.1, 0, 0, new CompassPIDSource(), new DummyPIDOutput());
    gyroPID = new PIDController(0.01, 0.0001, 0.00001, new GyroPIDSource(), new DummyPIDOutput());
    leftWheelsPID = new PIDController(0.02, 0.0004, 0, leftWheelsPIDSource, new DummyPIDOutput());
    rightWheelsPID = new PIDController(0.02, 0.0004, 0, rightWheelsPIDSource, new DummyPIDOutput());

    compassPID.disable();
    compassPID.setOutputRange(-1.0, 1.0); // Set turning speed range
    compassPID.setPercentTolerance(5.0); // Set tolerance of 5%

    gyroPID.disable();
    gyroPID.setOutputRange(-1.0, 1.0); // Set turning speed range
    gyroPID.setPercentTolerance(5.0); // Set tolerance of 5%

    leftWheelsPID.disable();
    leftWheelsPID.setOutputRange(-1.0, 1.0);

    rightWheelsPID.disable();
    rightWheelsPID.setOutputRange(-1.0, 1.0);
}

public void Init()
{
    Logger.PrintLine("Init 1",Logger.LOGGER_MESSAGE_LEVEL_DEBUG);

    m_pidController = new PIDController(10,10,10,m_encoderAverager,m_robotDrivePid);
    m_pidController.setOutputRange(-0.8,0.8);
    //m_pidController.setOutputRange(-0.4,0.4);
    Logger.PrintLine("Init 4",Logger.LOGGER_MESSAGE_LEVEL_ERROR);

    m_currentStateIndex = 0;
    SetCurrentState(m_nextStateArray[m_currentStateIndex]);
    m_disabled = false;

    m_shootingBall = false;
    m_driving = false;
    m_braking = false;
    m_detectingImage = false;
    m_currentStateIndex = 0;
    m_loadingBall = false;
    m_shooterPullingBack = false;

    m_autonomousStartTime = Timer.getFPGATimestamp();

    m_robotDrivePid.resetGyro();
}

public Drivetrain()
{
    backModule = new SwerveModule(RobotMap.backWheelEncoder, RobotMap.backModulePot, RobotMap.backWheelMotor, RobotMap.backModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, 0, -32.5, "backModule");
    leftModule = new SwerveModule(RobotMap.leftWheelEncoder, RobotMap.leftModulePot, RobotMap.leftWheelMotor, RobotMap.leftModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, -17.25, 0, "leftModule");
    rightModule = new SwerveModule(RobotMap.rightWheelEncoder, RobotMap.rightModulePot, RobotMap.rightWheelMotor, RobotMap.rightModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, 17.25, 0, "rightModule");
    swerves = new SwerveModule[3];
    swerves[0] = backModule;
    swerves[1] = leftModule;
    swerves[2] = rightModule;

    odometry = new Odometry();

    absoluteTwistPID = new PIDController(RobotMap.ABSOLUTE_TWIST_kP, RobotMap.ABSOLUTE_TWIST_kI, RobotMap.ABSOLUTE_TWIST_kD, RobotMap.navX, absoluteTwistPIDOutput);
    absoluteTwistPID.setInputRange(-180, 180);
    absoluteTwistPID.setOutputRange(-RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED);
    absoluteTwistPID.setContinuous();
    absoluteTwistPID.setAbsoluteTolerance(1);

    absoluteCrabPID = new PIDController(RobotMap.ABSOLUTE_CRAB_kP, RobotMap.ABSOLUTE_CRAB_kI, RobotMap.ABSOLUTE_CRAB_kD, odometry, absoluteCrabPIDOutput);
    absoluteCrabPID.setAbsoluteTolerance(.2);

    (new Thread(odometry)).start();
}

public Arm() {
    shoulder = new PIDController(RobotMap.ARM_SHOULDER_kP, RobotMap.ARM_SHOULDER_kI, RobotMap.ARM_SHOULDER_kD, RobotMap.armShoulderPot, RobotMap.armShoulderMotor, .02);
    shoulder.setInputRange(RobotMap.ARM_SHOULDER_MIN, RobotMap.ARM_SHOULDER_MAX);
    shoulder.setOutputRange(-RobotMap.ARM_MAX_SPEED, RobotMap.ARM_MAX_SPEED);
    shoulder.setAbsoluteTolerance(5);
    elbow = new PIDController(RobotMap.ARM_ELBOW_kP, RobotMap.ARM_ELBOW_kI, RobotMap.ARM_ELBOW_kD, RobotMap.armElbowPot, RobotMap.armElbowMotor, .02);
    elbow.setInputRange(-180, 180);
    elbow.setOutputRange(-RobotMap.ARM_MAX_SPEED, RobotMap.ARM_MAX_SPEED);
    elbow.setAbsoluteTolerance(5);
    wrist = new PIDController(RobotMap.ARM_WRIST_kP, RobotMap.ARM_WRIST_kI, RobotMap.ARM_WRIST_kD, RobotMap.armWristPot, RobotMap.armWristMotor, .02);
    wrist.setInputRange(-180, 180);
    wrist.setOutputRange(-RobotMap.ARM_MAX_SPEED, RobotMap.ARM_MAX_SPEED);
    wrist.setAbsoluteTolerance(5);

    LiveWindow.addActuator("Arm", "shoulder", shoulder);
    LiveWindow.addActuator("Arm", "elbow", elbow);
    LiveWindow.addActuator("Arm", "wrist", wrist);
}

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 void initialize() {
        pid = new PIDController(0.1, 0.1, 0, RobotMap.imu, new TurnController(), 0.01);

        startingAngle = RobotMap.imu.getYaw();
        double deltaAngle = angle + startingAngle;
//      deltaAngle %= 360;
        while (deltaAngle >= 180)
            deltaAngle -= 360;
        while (deltaAngle < -180)
            deltaAngle += 360;
        Preferences.getInstance().putDouble("YawSetpoint", deltaAngle);

        pid.setAbsoluteTolerance(2);
        pid.setInputRange(-180, 180);
        pid.setContinuous(true);
        pid.setOutputRange(-1 * rotateSpeed, 1 * rotateSpeed);
        pid.setSetpoint(deltaAngle);
        pid.enable();
        //SmartDashboard.putData("PID", pid);

    }

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
}

/**
 * Creates a new {@link MecanumDriveAlgorithm} that controls the specified
 * {@link FourWheelDriveController}.
 *
 * @param controller the {@link FourWheelDriveController} to control
 * @param gyro the {@link Gyro} to use for orientation correction and
 * field-oriented driving
 */
public MecanumDriveAlgorithm(FourWheelDriveController controller, Gyro gyro) {
    super(controller);
    // Necessary because we hide the controller field inherited from
    // DriveAlgorithm (if this was >=Java 5 I would use generics).
    this.controller = controller;
    this.gyro = gyro;
    rotationPIDController = new PIDController(
            ROTATION_P,
            ROTATION_I,
            ROTATION_D,
            ROTATION_F,
            gyro,
            new PIDOutput() {
                public void pidWrite(double output) {
                    rotationSpeedPID = output;
                }
            }
    );
    SmartDashboard.putData("Rotation PID Controller", rotationPIDController);
}

private boolean unwindWheel(AnalogChannelVolt wheel, PIDController pid) {
    double temp;
    double turns = wheel.getTurns();
    if (turns > 1) {
        temp = wheel.getAverageVoltage() - 1.0;
        if (temp < 0.0) {
            temp = 5.0 + temp;
        }
        pid.setSetpoint(temp);
        return true;
    } else if (turns < -1) {
        temp = wheel.getAverageVoltage() + 1.0;
        if (temp > 5.0) {
            temp = temp - 5.0;
        }
        pid.setSetpoint(temp);
        return true;
    } else {
        return false;
    }
}

/**
     * Constructs the command with the given sensor and range.
     * @param sensor the Sensor to read.
     * @param range the target value.
     */
    public DriveToRange(Ranger sensor, double range) {
        CompetitionRobot.output("Driving to range "+range);
        pid = Subsystems.pid;
        stablizer = new PIDController(0.04,0,0, Subsystems.imu, new PIDTurnInterface());
//      pid = new PIDController(01d,0,0,sensor,new PIDDriveInterface());
        targetRange = range;
//      pid.setOutputRange(-1*SPEED, SPEED);
        pid.setOutputRange(-1*SmartDashboard.getNumber("MaxApproachSpeed"), SmartDashboard.getNumber("MaxApproachSpeed"));
        pid.setInputRange(0, 2.5);
        pid.setPercentTolerance(1.0d);
        pid.setContinuous(false);
        stablizer.setOutputRange(-0.2, 0.2);
        stablizer.setInputRange(0,360);
        stablizer.setPercentTolerance(1.0);
        stablizer.setContinuous(true);
    }

private boolean unwindWheel(AnalogChannelVolt wheel, PIDController pid) {
    double temp;
    double turns = wheel.getTurns();
    if (turns > 1) {
        temp = wheel.getAverageVoltage() - 1.0;
        if (temp < 0.0) {
            temp = 5.0 + temp;
        }
        pid.setSetpoint(temp);
        return true;
    } else if (turns < -1) {
        temp = wheel.getAverageVoltage() + 1.0;
        if (temp > 5.0) {
            temp = temp - 5.0;
        }
        pid.setSetpoint(temp);
        return true;
    } else {
        return false;
    }
}

CrabDrive() throws CANTimeoutException
    {
        gyro = new GyroSensor(ReboundRumble.ROBOT_ANGLE_GYRO_SENSOR);
        front = new SteeringUnit(ReboundRumble.FRONT_STEERING_CAN_ID,
                ReboundRumble.FRONT_LEFT_CAN_ID,
                ReboundRumble.FRONT_RIGHT_CAN_ID);
        rear = new SteeringUnit(ReboundRumble.REAR_STEERING_CAN_ID,
                ReboundRumble.REAR_LEFT_CAN_ID,
                ReboundRumble.REAR_RIGHT_CAN_ID);
        turnController = new PIDController(TURN_CONTROLLER_PROPORTIONAL,
                TURN_CONTROLLER_INTEGRAL,
                TURN_CONTROLLER_DIFFERENTIAL,
                gyro,
                this);
        turnController.setOutputRange(MIN_OUTPUT, MAX_OUTPUT);
        turnController.disable();
//        leftBallSensor = new UltraSonicSensor(ReboundRumble.LEFT_BALL_RANGE_SENSOR);
//        rightBallSensor = new UltraSonicSensor(ReboundRumble.RIGHT_BALL_RANGE_SENSOR);
    }

/**
 * 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);
}

protected void initialize() {

    translator = new PIDOutputTranslator();

    //controller = new PIDController(0.00546875, 0, 0, driveTrain.gyro, translator);

    // 2/14/13 we tested and got: P 0.005554872047244094 I 2.8125000000000003E-4
    //THESE ARE SLIGHTLY TOO BIG!!!!!
    controller = new PIDController((oi.getDriveyStickThrottle() + 1.0) * 0.00546875, (oi.getLeftStickThrottle() + 1.0) * 0.001, 0, driveTrain.gyro, translator);
    // P = 0.00546875
    initialAngle = driveTrain.getGyroAngle();
    controller.setSetpoint(initialAngle + angle);
    controller.setPercentTolerance(1);
    controller.setInputRange(-360, 360);
    controller.enable();
}

/**
 *
 * @param theta degrees to rotate drive train.
 */
public RotateDriveTrain(double theta){
    requires(Robot.drivetrain);
    requires(Robot.driveEncoders);
    requires(Robot.gyro);
    this.theta=theta;
    pid = new PIDController(0.55, 0.002, 0.4, Robot.gyro.getGyro(), new pidOutput());
    pid.setAbsoluteTolerance(0.25);
    pid.setInputRange(-360,360);
    pid.setOutputRange(-0.65,0.65);
    pid.setSetpoint(theta);
}

/**
 * Requires gyro, ultrasonic, drive train
 * @param setpoint distance away in inches
 */
public DriveUntil(double setpoint){
    requires(Robot.drivetrain);
    requires(Robot.gyro);
    requires(Robot.ultrasonic);
    pid = new PIDController(0.27, 0, 0, Robot.ultrasonic.getUltrasonic(), new pidOutput());
    pid.setAbsoluteTolerance(2);
    pid.setSetpoint(setpoint);
}

public ArcDriveDistance(double radius, boolean isRighthand){
    requires(Robot.drivetrain);
    requires(Robot.driveEncoders);
    requires(Robot.gyro);
    requires(Robot.ultrasonic);
    if (isRighthand){
        setpoint1 = 2*Math.PI*radius*(1/4);
        setpoint2 = 2*Math.PI*(radius-34)*(1/4);
    }else{
        setpoint1 = 2*Math.PI*(radius-34)*(1/4);
        setpoint2 = 2*Math.PI*radius*(1/4);
    }
    pidX= new PIDController(0.05, 0, 0, Robot.driveEncoders.getEncoderLeft(), new pidXOutput());
    if(setpoint1>0){
        pidX.setInputRange(0,setpoint1+10);
    }else{
        pidX.setInputRange(setpoint1-10,0);
    }
    pidX.setAbsoluteTolerance(0.5);
    pidX.setOutputRange(-0.6,0.6);
    pidX.setSetpoint(setpoint1);

    pidY = new PIDController(0.05, 0, 0, Robot.driveEncoders.getEncoderRight(), new pidYOutput());
    if(setpoint2>0){
        pidY.setInputRange(0,setpoint2+10);
    }else{
        pidY.setInputRange(setpoint2-10,0);
    }
    pidY.setAbsoluteTolerance(0.5);
    pidY.setOutputRange(-0.6,0.6);
    pidY.setSetpoint(setpoint2);
}

/**
 * Requires flywheel, meter
 * @param rpm revs per min to spin shooter to
 */
public SpinRPM(double rpm){
    rpm = rpm/60;
    requires(Robot.flywheel);
    requires(Robot.meter);
    enc.setMaxPeriod(0.1);
    enc.setMinRate(10);
    enc.setDistancePerPulse(0.05);
    enc.setReverseDirection(true);
    enc.setSamplesToAverage(7);
    pid = new PIDController(0.27, 0, 0, enc, new pidOutput());
    pid.setAbsoluteTolerance(100);
    pid.setSetpoint(rpm);
    pid.setContinuous();
}

/**
 * Requires DriveTrain, DriveEncoders, Gyro, Ultrasonic
 * @param setpoint distance to drive in inches
 */
public DriveDistanceFast(double setpoint){
    requires(Robot.drivetrain);
    requires(Robot.driveEncoders);
    requires(Robot.gyro);
    requires(Robot.ultrasonic);
    this.setpoint=setpoint;
    pid = new PIDController(0.27, 0, 0, Robot.driveEncoders.getEncoderLeft(), new pidOutput());
    pid.setAbsoluteTolerance(1);
    pid.setSetpoint(setpoint);
}

public RotateWithPIDTankDrive(double targetAngleDegrees) {
      requires(Robot.driveTrain);

/*      try {
            *//***********************************************************************
             * navX-MXP: - This is all defined in NavX Subsystem.
             * - Communication via RoboRIO MXP (SPI, I2C, TTL UART) and USB.            
             * - See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface.
             * 
             * navX-Micro:
             * - Communication via I2C (RoboRIO MXP or Onboard) and USB.
             * - See http://navx-micro.kauailabs.com/guidance/selecting-an-interface.
             * 
             * Multiple navX-model devices on a single robot are supported.
             ************************************************************************//*

      //ahrs = new AHRS(SerialPort.Port.kUSB); 
      } catch (RuntimeException ex ) {
          DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
      }*/

      this.targetAngleDegrees=targetAngleDegrees;
      turnController2 = new PIDController(kP, kI, kD, kF, NavX.ahrs, this);
      turnController2.setInputRange(-180.0f,  180.0f);
      //turnController2.setOutputRange(-1.0, 1.0);
      turnController2.setOutputRange(-0.7, 0.7);
      turnController2.setAbsoluteTolerance(kToleranceDegrees);
      turnController2.setContinuous(true);
      turnController2.disable();
    }

@Override
protected void initDefaultCommand() {
    turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
    turnController.setInputRange(-180.0f,  180.0f);
    turnController.setOutputRange(-1.0, 1.0);
    turnController.setAbsoluteTolerance(kToleranceDegrees);
    turnController.setContinuous(true);

}

public SwerveModule(CANTalon d, CANTalon a, AnalogInput e, String name)
{
    SpeedP = Config.getSetting("speedP",1);
    SpeedI = Config.getSetting("speedI",0);
    SpeedD = Config.getSetting("speedD",0);
    SteerP = Config.getSetting("steerP",2);
    SteerI = Config.getSetting("steerI",0);
    SteerD = Config.getSetting("steerD",0);
    SteerTolerance = Config.getSetting("SteeringTolerance", .25);
    SteerSpeed = Config.getSetting("SteerSpeed", 1);
    SteerEncMax = Config.getSetting("SteerEncMax",4.792);
    SteerEncMax = Config.getSetting("SteerEncMin",0.01);
    SteerOffset = Config.getSetting("SteerEncOffset",0);
    MaxRPM = Config.getSetting("driveCIMmaxRPM", 4200);

    drive = d;
    drive.setPID(SpeedP, SpeedI, SpeedD);
    drive.setFeedbackDevice(FeedbackDevice.QuadEncoder);
   drive.configEncoderCodesPerRev(20);
   drive.enable();

    angle = a;

    encoder = e;

    encFake = new FakePIDSource(SteerOffset,SteerEncMin,SteerEncMax);

    PIDc = new PIDController(SteerP,SteerI,SteerD,encFake,angle);
    PIDc.disable();
    PIDc.setContinuous(true);
    PIDc.setInputRange(SteerEncMin,SteerEncMax);
    PIDc.setOutputRange(-SteerSpeed,SteerSpeed);
    PIDc.setPercentTolerance(SteerTolerance);
    PIDc.setSetpoint(2.4);
    PIDc.enable();
    s = name;
}

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 GyroPID() {
    gyroSource = new GyroSource();
    defaultOutput = new DefaultOutput();

    gyroPID = new PIDController(Constants.GYRO_P, Constants.GYRO_I, Constants.GYRO_D, gyroSource, defaultOutput);
    gyroPID.setContinuous();
    gyroPID.setOutputRange(-Constants.GYRO_CAP, Constants.GYRO_CAP);
    gyroPID.enable();

    SmartDashboard.putData("GryoPID", gyroPID);
}

public VisionAreaPID() {
    visionAreaSource = new VisionAreaSource();
    defaultOutput = new DefaultOutput();

    areaPID = new PIDController(Constants.AREA_P, Constants.AREA_I, Constants.AREA_D, visionAreaSource, defaultOutput);
    areaPID.setContinuous();
    areaPID.setOutputRange(-Constants.AREA_CAP, Constants.AREA_CAP);
    areaPID.enable();

    SmartDashboard.putData("AreaPID", areaPID);
}

public VisionOffsetPID() {
    visionOffsetSource = new VisionOffsetSource();
    defaultOutput = new DefaultOutput();

    offsetPID = new PIDController(Constants.OFFSET_P, Constants.OFFSET_I, Constants.OFFSET_D, visionOffsetSource, defaultOutput);
    offsetPID.setSetpoint(0);
    offsetPID.setContinuous();
    offsetPID.setOutputRange(-Constants.OFFSET_CAP, Constants.OFFSET_CAP);
    offsetPID.enable();
    SmartDashboard.putData("OffsetPID", offsetPID);
}

public UltraDrive(double dist) {
    requires(drivebase);
    distance = dist;
    drive = new PIDRobotDrive(drivebase.robotdrive, false);
    PID = new PIDController(0.1, 0.0, 0.0, drivebase.ultrasonic, drive);
    PID.setAbsoluteTolerance(5.0);
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
}

public GyroTurn(double a){
    requires(drivebase);
    angle = a;
    gyro = new Pigeon(drivebase.pigeon);
    drive = new PIDRobotDrive(drivebase.robotdrive, true);
    PID = new PIDController(0.1, 0.0, 0.0, gyro, drive);
    PID.setAbsoluteTolerance(.5);
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
}

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);
}

public Drivetrain() {
    //TODO: Init Gyro gyro = new Gyro();
    pidOutput = new DrivetrainOutput(this);
    pidInput = new GyroInput(gyro);
    controller = new PIDController(0.0,0.0,0.0, pidInput, pidOutput);

    //super(Kp, Ki, Kd);
    leftFrontMotor = new CANTalon(RobotMap.LEFT_FRONT_MOTOR_PORT);
    rightFrontMotor = new CANTalon(RobotMap.RIGHT_FRONT_MOTOR_PORT);
    leftBackMotor = new CANTalon(RobotMap.LEFT_BACK_MOTOR_PORT);
    rightBackMotor = new CANTalon(RobotMap.RIGHT_BACK_MOTOR_PORT);
    //Assume the team does not have encoders if they do not have a mobility auton command.
    robotDrive = new RobotDrive(leftBackMotor, leftFrontMotor, rightBackMotor, rightFrontMotor);
}