implement ToggleWinch command

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index a1a8cb075cf635e8db11e774f068d0a2722620e9..3567e2f7588937413b548f7b882c7c8563d7dc72 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -45,8 +45,6 @@ public class Constants {
     public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
     public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
 
-    public static final double CLIMBER_SPEED = 5;
-
     // MOTOR CONTROLLERS
     public static final int FRONT_LEFT = 1;
     public static final int FRONT_RIGHT = 3;

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index 05fb72dbbbf46fc6c1bad4c9df8adbddc679354b..f878e7cf57016d154da3f7c85ac3cbefed482210 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,6 +1,6 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commandgroups.ToggleWinch;
+import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch;
 import org.usfirst.frc.team3501.robot.commands.driving.ToggleGear;
 import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
 import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous;
@@ -57,12 +57,6 @@ public class OI {
 
     toggleWinch = new JoystickButton(leftJoystick,
         Constants.OI.TOGGLE_WINCH_PORT);
-    /*
-     * if (!Robot.getDriveTrain().isClimbing()) { toggleWinch.whenPressed(new
-     * RunWinchContinuous()); Robot.getDriveTrain().setClimbing(true); } else {
-     * toggleWinch.whenPressed(new MaintainClimbedPosition());
-     * Robot.getDriveTrain().setClimbing(false); }
-     */
     toggleWinch.whenPressed(new ToggleWinch());
   }
 

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java
deleted file mode 100644 (file)
index 3766aee..0000000
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java
+++ /dev/null
@@ -1,22 +0,0 @@
-package org.usfirst.frc.team3501.robot.commandgroups;
-
-import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
-import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-public class ToggleWinch extends CommandGroup {
-
-  public ToggleWinch() {
-    if (!Robot.getDriveTrain().isClimbing()) {
-      Robot.getDriveTrain().setClimbing(true);
-      addSequential(new RunWinchContinuous());
-
-    } else {
-      Robot.getDriveTrain().setClimbing(false);
-      addSequential(new MaintainClimbedPosition());
-    }
-  }
-
-}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java
index 65277997b70fd20ccad3d85fbe60b2b5a8eb23c1..210199d86e2cb3dc178fb5d289f0446e4147368f 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot.commands.climber;
 
 import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -17,6 +18,8 @@ import edu.wpi.first.wpilibj.command.Command;
  *
  */
 public class RunWinchContinuous extends Command {
+  DriveTrain driveTrain = Robot.getDriveTrain();
+  private double climbingSpeed;
 
   /**
    * See JavaDoc comment in class for details
@@ -25,19 +28,17 @@ public class RunWinchContinuous extends Command {
    *          value range is from -1 to 1
    */
   public RunWinchContinuous() {
-    requires(Robot.getDriveTrain());
+    requires(driveTrain);
+    climbingSpeed = driveTrain.CLIMBER_SPEED;
   }
 
   @Override
   protected void initialize() {
-    Robot.getDriveTrain().setMotorValues(
-        Robot.getDriveTrain().getClimbingSpeed(),
-        Robot.getDriveTrain().getClimbingSpeed());
   }
 
   @Override
   protected void execute() {
-
+    Robot.getDriveTrain().setMotorValues(climbingSpeed, climbingSpeed);
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
new file mode 100644 (file)
index 0000000..3e60406
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team3501.robot.commands.climber;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToggleWinch extends Command {
+  DriveTrain driveTrain = Robot.getDriveTrain();
+  private double climbingSpeed;
+  private double maintainPositionSpeed;
+
+  public ToggleWinch() {
+    requires(driveTrain);
+    climbingSpeed = driveTrain.CLIMBER_SPEED;
+    maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION;
+  }
+
+  @Override
+  protected void initialize() {
+  }
+
+  @Override
+  protected void execute() {
+    if (driveTrain.shouldBeClimbing) {
+      driveTrain.setMotorValues(climbingSpeed, climbingSpeed);
+    } else {
+      driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed);
+    }
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  protected void end() {
+    driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing;
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 7463b8685faa504319777e830231c066b8088154..b0113e8ed66312fc74931f0b79c101b56f08c4f8 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -21,8 +21,11 @@ public class DriveTrain extends Subsystem {
   public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
   public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
       / ENCODER_PULSES_PER_REVOLUTION;
-  public static final int MAINTAIN_CLIMBED_POSITION = 0;
-  public static final int TIME_TO_CLIMB_FOR = 0;
+
+  public static final double MAINTAIN_CLIMBED_POSITION = 0;
+  public static final double TIME_TO_CLIMB_FOR = 0;
+  public static final double CLIMBER_SPEED = 0;
+
   private static DriveTrain driveTrain;
 
   private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
@@ -32,8 +35,7 @@ public class DriveTrain extends Subsystem {
 
   private ADXRS450_Gyro imu;
 
-  private boolean isClimbing;
-  private static double CLIMBER_SPEED;;
+  public boolean shouldBeClimbing;
 
   private DriveTrain() {
     // MOTOR CONTROLLERS
@@ -63,8 +65,6 @@ public class DriveTrain extends Subsystem {
     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
         Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
         Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
-
-    CLIMBER_SPEED = Constants.DriveTrain.CLIMBER_SPEED;
   }
 
   public static DriveTrain getDriveTrain() {
@@ -81,7 +81,6 @@ public class DriveTrain extends Subsystem {
 
     frontRight.set(-right);
     rearRight.set(-right);
-    this.isClimbing = true;
   }
 
   public void joystickDrive(final double thrust, final double twist) {
@@ -90,7 +89,6 @@ public class DriveTrain extends Subsystem {
 
   public void stop() {
     setMotorValues(0, 0);
-    this.isClimbing = false;
   }
 
   public double getLeftMotorVal() {
@@ -189,17 +187,4 @@ public class DriveTrain extends Subsystem {
   protected void initDefaultCommand() {
     setDefaultCommand(new JoystickDrive());
   }
-
-  public boolean isClimbing() {
-    return this.isClimbing;
-  }
-
-  public void setClimbing(boolean isClimbing) {
-    this.isClimbing = isClimbing;
-  }
-
-  public double getClimbingSpeed() {
-    return this.CLIMBER_SPEED;
-  }
-
 }