Java PIDController类代码示例

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
/**
 * 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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
/**
 * 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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
/**
 * 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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
/**
 * 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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
/**
* 
* @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);
  }
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
/**
* 
* @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);
  }
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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?
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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();
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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();
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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);
}
 

import edu.wpi.first.wpilibj.PIDController; //导入依赖的package包/类
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);
    
    }