protected boolean isFinished() {
if (drivebase.onTarget() || isTimedOut()) {
// Disable PID controller
m_pc.disable();
return true;
} else return false;
}
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 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 void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
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();
}
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
}
public void autonomousContinuous() {
long currentTime = System.currentTimeMillis();
long timeSinceLastT = currentTime - lastTtime;
// If we're not at a T, continue the loop
if (!robot.lineSensor.isAtT()) return;
System.out.println("At a T");
lastTtime = currentTime;
// If we're seeing the same T, continue the loop
if (timeSinceLastT < T_WAIT_TIME) return;
tCount++;
// If this isn't the last T, continue the loop
if (tCount != NUM_TS) return;
controller.disable();
robot.grabber.release();
// Arm goes down
}
public static void pidDisable() {
controller.disable();
}
public void disabledInit() {
controller.disable();
}
// Called once after isFinished returns true
protected void end() {
// Stop PID and the wheels
pid.disable();
Robot.drivetrain.drive(0, 0);
}
public void teleopPeriodic() {
/*SmartDashboard.getNumber("Accel x: ", accel.getX());
SmartDashboard.getNumber("Accel Y: ", accel.getY());
SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
if (controller.isEnabled()) {
if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
}
if (j1.getRawButton(2)) {
enc.reset();
controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value
timerStart = Timer.getFPGATimestamp();
timeToCancel = 10; // timeout after 10 seconds
controller.enable();
} else if (j1.getRawButton(
1)) { // this button stops the robot if the button 2 command goes crazy
controller.disable();
timerStart = -1;
timeToCancel = -1;
} else { // if time out or distance, end
controller.disable();
timerStart = -1;
timeToCancel = -1;
}
if (!controller.isEnabled()) {
rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
}
if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
shooter.set(-j2.getRawAxis(2));
shooter2.set(-j2.getRawAxis(2));
SmartDashboard.putNumber("Shooter: ", shooter.get());
} else { // stop shooter
shooter.set(0);
shooter2.set(0);
}
/*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
shooter.set(-1);
intake.set(.5);
Timer.delay(.50);
intake.set(0);
Timer.delay(1.5);
intake.set(-1);
Timer.delay(1);
shooter.set(0);
intake.set(0);
}*/
// Need to mess around with sensitivity of light sensor
if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
intake.set(-1 * j2.getRawAxis(3));
} else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
intake.set(-1);
} else if (j2.getRawButton(6)) { // run intake out of robot
intake.set(1);
} else { // stop intake
intake.set(0);
}
if (j2.getRawButton(2)) { // lift intake
inup.set(DoubleSolenoid.Value.kForward);
inup2.set(DoubleSolenoid.Value.kForward);
} else if (j2.getRawButton(3)) { // drop intake
inup.set(DoubleSolenoid.Value.kReverse);
inup2.set(DoubleSolenoid.Value.kReverse);
} else { // solenoids off
inup.set(DoubleSolenoid.Value.kOff);
inup2.set(DoubleSolenoid.Value.kOff);
}
// reading values
SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
SmartDashboard.putNumber("Encoder: ", enc.get());
SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
SmartDashboard.putBoolean("Light Sensor: ", light.get());
}
protected void end() {
pidControllerDistance.disable();
drivetrainStrafe.mecanumPolarStrafe(0, 0);
drivetrainRotation.mecanumPolarRotate(0);
timer.stop();
}
// Called once after isFinished returns true
protected void end() {
// Disable PID output and stop robot to be safe
leftPID.disable();
rightPID.disable();
Robot.driveTrain.drive(0, 0);
}
public void stopBalancing() {
enabled = false;
controller.disable();
}