// Called just before this Command runs the first time
protected void initialize() {
Robot.driveTrain.reset();
// Make input infinite
leftPID.setContinuous();
rightPID.setContinuous();
// Set output speed range
if (speed > 0) {
leftPID.setOutputRange(-speed, speed);
rightPID.setOutputRange(-speed, speed);
} else {
leftPID.setOutputRange(speed, -speed);
rightPID.setOutputRange(speed, -speed);
}
Robot.driveTrain.initGyro = Robot.driveTrain.getHeading();
leftPID.setSetpoint(distance);
rightPID.setSetpoint(distance);
// Will accept within 5 inch of target
leftPID.setAbsoluteTolerance(5.0 / 12);
rightPID.setAbsoluteTolerance(5.0 / 12);
// Start going to location
leftPID.enable();
// rightPID.enable();
}
public ElevatorPID(Robot robot) {
this.robot = robot;
elevatorPot = new AnalogPotentiometer(Constants.elevatorPot);
elevatorPID = new PIDController(p, i, d, this, robot.elevator.elevatorSingle);
elevatorPID.setAbsoluteTolerance(Constants.elevatorPIDTolerance);
}
static {
// stealerEncoder.setDistancePerPulse(.0245);
stealerEncoder.setMaxPeriod(1 /* seconds */);
stealerEncoder.reset();
lassoCounter.reset();
controller.setOutputRange(-0.2, 0.2);
controller.setAbsoluteTolerance(50);
}
public void robotInit() {
j1 = new Joystick(1);
j2 = new Joystick(0);
shooter = new Victor(2);
shooter2 = new Victor(4);
intake = new Victor(3);
right = new Victor(0);
left = new Victor(1);
rd = new RobotDrive(left, right);
light = new DigitalInput(3);
count = 0;
comp = new Compressor();
inup = new DoubleSolenoid(0, 1);
inup2 = new DoubleSolenoid(2, 3);
gyro = new AnalogGyro(0);
gyro.setSensitivity(.007);
gyro.calibrate();
gyro.reset();
enc = new Encoder(1, 2, true, Encoder.EncodingType.k4X);
enc.setDistancePerPulse(.11977); // circumference of wheel/200 (PPR)
enc.setPIDSourceType(PIDSourceType.kDisplacement);
enc.reset();
out =
new PIDOutput() {
@Override
public void pidWrite(double out) {
rd.setLeftRightMotorOutputs(out, out);
}
};
controller = new PIDController(.007, 0, 0, enc, out);
controller.setAbsoluteTolerance(.25);
controller.disable();
gcontroller = new PIDController(kp, ki, kd, gyro, out);
gcontroller.setAbsoluteTolerance(ktol);
gcontroller.disable();
timerStart = -1;
timeToCancel = -1;
}
public DriveStraight(double distance) {
requires(Robot.drivetrain);
pid =
new PIDController(
4,
0,
0,
new PIDSource() {
public double pidGet() {
return Robot.drivetrain.getDistance();
}
},
new PIDOutput() {
public void pidWrite(double d) {
Robot.drivetrain.drive(d, d);
}
});
pid.setAbsoluteTolerance(0.01);
pid.setSetpoint(distance);
}
protected void initialize() {
// Use standard mecanum drive
drivebase.initMecanum(false);
/*
* Enable drivebase rotational PID
*/
// Set target angle
m_pc.setSetpoint(m_target);
// Throttle down the max allowed speed
m_pc.setOutputRange(-0.5, 0.5);
// Set tolerance around the target angle
m_pc.setAbsoluteTolerance(5);
// Reset and enable PID controller
m_pc.reset();
m_pc.enable();
}