// 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();
}
static {
// stealerEncoder.setDistancePerPulse(.0245);
stealerEncoder.setMaxPeriod(1 /* seconds */);
stealerEncoder.reset();
lassoCounter.reset();
controller.setOutputRange(-0.2, 0.2);
controller.setAbsoluteTolerance(50);
}
public boolean elevatePIDTuning(double height, double speed) {
elevatorPID.setOutputRange(-0.5, speed);
elevatorPID.setSetpoint(height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorVics.set(0);
return true;
} else {
return false;
}
}
public boolean elevatePID(double height) {
elevatorPID.setOutputRange(-1, 1);
elevatorPID.setSetpoint(-height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorSingle.set(0);
return true;
} else {
return false;
}
}
public Balancer(Drivetrain robot) {
this.robot = robot;
sensor = robot.getGyro();
// set up the pid controller - input to pid is the gyro degrees, output is motor PWM signal
controller = new PIDController(P, I, D, sensor, robot);
controller.setTolerance(
tolerance); // must set these values so that the pid has a frame of reference.
controller.setInputRange(-maxDeg, maxDeg);
controller.setOutputRange(-1, 1);
// register with config file
Configuration.getInstance().register("Balancer", this);
}
public boolean elevatePIDLock(double height) {
if (robot.elevator.locked) {
robot.elevator.elevatorAutoRelease();
}
elevatorPID.setOutputRange(-1, 1);
elevatorPID.setSetpoint(height);
elevatorPID.enable();
if (elevatorPID.onTarget()) {
elevatorPID.disable();
robot.elevator.elevatorVics.set(0);
if (height > 0) {
robot.elevator.elevatorLock();
}
}
return elevatorPID.onTarget();
}
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();
}
public DriveTrain() {
// super supplies the PID constants (.05, 0, 0)
super("DriveTrain", .0070, .0003, 0);
robotDrive = new RobotDrive(_leftMaster, _rightMaster);
// Invert the appropriate controllers
// robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
_leftSlave1.changeControlMode(TalonControlMode.Follower);
_leftSlave1.set(_leftMaster.getDeviceID());
_leftSlave2.changeControlMode(TalonControlMode.Follower);
_leftSlave2.set(_leftMaster.getDeviceID());
_rightSlave1.changeControlMode(TalonControlMode.Follower);
_rightSlave1.set(_rightMaster.getDeviceID());
_rightSlave2.changeControlMode(TalonControlMode.Follower);
_rightSlave2.set(_rightMaster.getDeviceID());
ahrs = Robot.ahrs;
// getPIDController() returns the PID Controller that is included with
// classes that extend PIDSubsystems.
turnController = getPIDController();
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-.7, .7);
// begin vision test
// frame = RobotMap.FRAME;
// session = RobotMap.CAMERA_SESSION;
// NIVision.IMAQdxConfigureGrab(session);
// colorTable = new NIVision.RawData();
// end vision test
}
/** This function is called periodically during operator control */
public void teleopInit() {
armpid.setInputRange(-180, 180);
armpid.setOutputRange(-1, 1);
armpid.setSetpoint(-22);
}
