1 package org
.usfirst
.frc
.team3501
.robot
.commands
.driving
;
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
;
7 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
.Value
;
8 import edu
.wpi
.first
.wpilibj
.command
.Command
;
11 * This command toggles the gear(low or high).
13 * post-condition: if the drivetrain is running at high gear, it will be made to
14 * run at low gear, and vice versa
16 public class ToggleDriveGear
extends Command
{
17 DriveTrain driveTrain
= Robot
.getDriveTrain();
19 public ToggleDriveGear() {
24 protected void initialize() {
29 protected void execute() {
30 Value rightPistonValue
= driveTrain
.getRightDriveTrainPiston();
31 System
.out
.println("I think i'm at " + rightPistonValue
);
32 if (rightPistonValue
== Constants
.DriveTrain
.REVERSE_PISTON_VALUE
) {
33 driveTrain
.setHighGear();
35 driveTrain
.setLowGear();
37 // boolean leftPistonValue = driveTrain.getLeftDriveTrainPiston();
38 // if (leftPistonValue == Constants.DriveTrain.RETRACT_VALUE) {
39 // driveTrain.setHighGear();
41 // driveTrain.setLowGear();
46 protected boolean isFinished() {
51 protected void end() {
55 protected void interrupted() {