add necessary commands/buttons
[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commands / driving / ToggleDriveGear.java
1 package org.usfirst.frc.team3501.robot.commands.driving;
2
3 import org.usfirst.frc.team3501.robot.Constants;
4 import org.usfirst.frc.team3501.robot.Robot;
5 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
6
7 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
8 import edu.wpi.first.wpilibj.command.Command;
9
10 /**
11 * This command toggles the gear(low or high).
12 *
13 * post-condition: if the drivetrain is running at high gear, it will be made to
14 * run at low gear, and vice versa
15 */
16 public class ToggleDriveGear extends Command {
17 DriveTrain driveTrain = Robot.getDriveTrain();
18
19 public ToggleDriveGear() {
20 requires(driveTrain);
21 }
22
23 @Override
24 protected void initialize() {
25
26 }
27
28 @Override
29 protected void execute() {
30 Value leftPistonValue = driveTrain.getLeftDriveTrainPiston();
31 Value rightPistonValue = driveTrain.getRightDriveTrainPiston();
32 if (leftPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) {
33 driveTrain.setHighGear();
34 } else {
35 driveTrain.setLowGear();
36 }
37 }
38
39 @Override
40 protected boolean isFinished() {
41 return true;
42 }
43
44 @Override
45 protected void end() {
46 }
47
48 @Override
49 protected void interrupted() {
50 }
51 }