Saved

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commands / climber / ToggleWinch.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
index 3e60406cc66e332167c465d59dae56d08311b13f..918481c37e2377a76ba5b5c2bca1f8a159b02a07 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
@@ -1,22 +1,17 @@
 package org.usfirst.frc.team3501.robot.commands.climber;
 
 import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
 
 import edu.wpi.first.wpilibj.command.Command;
 
-/**
- *
- */
 public class ToggleWinch extends Command {
-  DriveTrain driveTrain = Robot.getDriveTrain();
+  Climber climber = Robot.getClimber();
   private double climbingSpeed;
-  private double maintainPositionSpeed;
 
   public ToggleWinch() {
-    requires(driveTrain);
-    climbingSpeed = driveTrain.CLIMBER_SPEED;
-    maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION;
+    requires(climber);
+    climbingSpeed = climber.CLIMBER_SPEED;
   }
 
   @Override
@@ -25,10 +20,15 @@ public class ToggleWinch extends Command {
 
   @Override
   protected void execute() {
-    if (driveTrain.shouldBeClimbing) {
-      driveTrain.setMotorValues(climbingSpeed, climbingSpeed);
+    if (climber.shouldBeClimbing) {
+      climber.setCANTalonsBrakeMode(climber.COAST_MODE);
+      climber.setMotorValues(climbingSpeed);
     } else {
-      driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed);
+      climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+
+      /* Not sure if should have */
+      climber.stop();
+      end();
     }
   }
 
@@ -39,7 +39,7 @@ public class ToggleWinch extends Command {
 
   @Override
   protected void end() {
-    driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing;
+    climber.shouldBeClimbing = !climber.shouldBeClimbing;
   }
 
   @Override