Java PIDSource类代码示例

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

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

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

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

import edu.wpi.first.wpilibj.PIDSource; //导入依赖的package包/类
public NavXGyroWrapper(PIDSource wrappedSource) throws IllegalSourceException {
	super(wrappedSource);
	if (wrappedSource.getClass() != AHRS.class) {
		throw new IllegalSourceException();
	}
	else {
		m_navxSource = ((AHRS) m_pidSource);
	}
}
 

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

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

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

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

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

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

import edu.wpi.first.wpilibj.PIDSource; //导入依赖的package包/类
public PIDSource getSource(final int index) {
    return new PIDSource() {
        public double pidGet() {
            return vals[index];
        }
    };
}
 

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