public void leftOpen(boolean open) {
if (!open) {
leftGate.set(DoubleSolenoid.Value.kReverse);
} else {
leftGate.set(DoubleSolenoid.Value.kForward);
}
}
// Drive Train
public void switchGears(DoubleSolenoid gearSwitch, boolean lowHigh) {
if (lowHigh) {
gearSwitch.set(DoubleSolenoid.Value.kForward);
}
if (!lowHigh) {
gearSwitch.set(DoubleSolenoid.Value.kReverse);
}
}
// Spinny Sticks
public void spinnySticksMovement(DoubleSolenoid spinnyStickSolenoid, boolean forwardBackward) {
if (forwardBackward) {
spinnyStickSolenoid.set(DoubleSolenoid.Value.kForward);
}
if (!forwardBackward) {
spinnyStickSolenoid.set(DoubleSolenoid.Value.kReverse);
}
}
public void periodic() {
if (raiseDragonTailButton.isSwitchOn()) {
dragonTailSolenoid.set(DoubleSolenoid.Value.kReverse);
timeLimit.setTimeLimit(SOLENOID_ON_LAG);
} else if (lowerDragonTailButton.isSwitchOn()) {
dragonTailSolenoid.set(DoubleSolenoid.Value.kForward);
timeLimit.setTimeLimit(SOLENOID_ON_LAG);
} else if (timeLimit.isTimeLimitReached()) {
dragonTailSolenoid.set(DoubleSolenoid.Value.kOff);
}
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
chooser = new SendableChooser();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto choices", chooser);
controlMethod = new SendableChooser();
controlMethod.addDefault("Tank Control", tankControl);
controlMethod.addObject("Stick Control", stickControl);
controlMethod.addObject("Clayton Control", claytonControl);
SmartDashboard.putData("Control method", controlMethod);
rightMotor = new CANTalon(RIGHT_INDEX);
rightSlave = new CANTalon(RIGHT_INDEX + 1);
rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
rightSlave.set(RIGHT_INDEX);
rightMotor.setInverted(true);
leftMotor = new CANTalon(LEFT_INDEX);
leftSlave = new CANTalon(LEFT_INDEX + 1);
leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
leftSlave.set(LEFT_INDEX);
intakeMotor = new CANTalon(INTAKE_INDEX);
// Arm motor is currently disabled
// armMotor = new CANTalon(ARM_INDEX);
compressor = new Compressor(0);
compressor.setClosedLoopControl(true);
intakePneumatic = new DoubleSolenoid(1, 2);
intakePneumatic.set(DoubleSolenoid.Value.kOff);
gamepad = new Gamepad(0);
}
public boolean hasShot() {
if (kickerLatch.get() == Value.kForward) {
return true;
} else {
return false;
}
}
public void set(DoubleSolenoid.Value value) {
piston.set(value);
if (value == extend) {
SmartDashboard.putString("Loader", "Extend");
} else if (value == retract) {
SmartDashboard.putString("Loader", "Retract");
} else {
SmartDashboard.putString("Loader", "Off");
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
MotorOutput motorOutput = drivetrainJoystick.motionForGamepadInput(gamepad);
leftMotor.set(motorOutput.left * MAX_SPEED);
rightMotor.set(motorOutput.right * MAX_SPEED);
double in = gamepad.getLeftTrigger();
double out = gamepad.getRightTrigger();
intakeMotor.set((in - out) * INTAKE_SPEED);
double fore = gamepad.getButtonLeftBack() ? 1 : 0;
double back = gamepad.getButtonRightBack() ? 1 : 0;
// Don't run the arm motor because it's not properly fastened
// armMotor.set((fore - back) * ARM_SPEED);
if (gamepad.getButtonA()) {
intakePneumatic.set(DoubleSolenoid.Value.kForward);
} else if (gamepad.getButtonB()) {
intakePneumatic.set(DoubleSolenoid.Value.kReverse);
} else {
intakePneumatic.set(DoubleSolenoid.Value.kOff);
}
}
public ShooterSubsystem() {
kickerLatch = new DoubleSolenoid(RobotMap.KICKER_LATCH_RELEASE, RobotMap.KICKER_LATCH_RELOAD);
kickerLatch.set(DoubleSolenoid.Value.kForward);
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon = new Talon(RobotMap.KICKER_MOTOR);
kickerTalon.set(0.0);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar = new Jaguar(RobotMap.KICKER_MOTOR);
kickerJaguar.set(0.0);
}
kickerLimitSwitch = new DigitalInput(RobotMap.KICKER_RETRACTED_SWITCH);
kickerMotorReturnSwitch = new DigitalInput(RobotMap.KICKER_MOTOR_RETURN_SWITCH);
}
public void retractArm() {
feederArmPiston.set(DoubleSolenoid.Value.kReverse);
}
public void armOff() {
feederArmPiston.set(DoubleSolenoid.Value.kOff);
}
public void lowerLift() {
liftValve2.set(Value.kReverse);
}
public void extendArm() {
feederArmPiston.set(DoubleSolenoid.Value.kForward);
}
public void closeIntake() {
pneuIntake.set(DoubleSolenoid.Value.kReverse); // change direction of intake solenoid to close
}
public void raiseLift() {
liftValve2.set(Value.kForward);
}
public void closeClaw() {
clawPneu.set(DoubleSolenoid.Value.kReverse); // change direction of claw solenoid to close
}
public void openIntake() {
intakeSolendoid.set(Value.kForward);
}
public void closeIntake() {
intakeSolendoid.set(Value.kReverse);
}
/** Engage the hanger / retract the actuators. */
public void engage() {
// TODO: Verify that kForward engages the hanger
// pull the hanger down to lift off the ground
actuator.set(DoubleSolenoid.Value.kReverse);
}
/** Disengage the hanger / extend the actuators */
public void disengage() {
// TODO: Verify that kForward disengages the hanger
// puts the hanger up to grab the bar
actuator.set(DoubleSolenoid.Value.kForward);
}
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());
}
public void shoot() {
kickerLatch.set(Value.kReverse);
}
public void stopSpinnySticksMovement(DoubleSolenoid spinnyStickSolenoid) {
spinnyStickSolenoid.set(DoubleSolenoid.Value.kOff);
}
public void unshoot() {
kickerLatch.set(Value.kForward);
}
public void openClaw() {
clawPneu.set(DoubleSolenoid.Value.kForward); // change direction of claw solenoid to open
}
public void openActuator() {
leftPiston.set(DoubleSolenoid.Value.kForward);
rightPiston.set(DoubleSolenoid.Value.kForward);
}
public void openIntake() {
pneuIntake.set(DoubleSolenoid.Value.kForward); // change direction of intake solenoid to open
}
public void closeActuator() {
leftPiston.set(DoubleSolenoid.Value.kReverse);
rightPiston.set(DoubleSolenoid.Value.kReverse);
}
public void runPunch(Value punchPower) {
punch.set(punchPower);
}
public void ShelfClose() {
LShelf.set(Value.kForward);
}