X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fclimber%2FToggleWinch.java;h=918481c37e2377a76ba5b5c2bca1f8a159b02a07;hb=8275a069e1891bd27156dcd947a897519c42a3a3;hp=3e60406cc66e332167c465d59dae56d08311b13f;hpb=9dc69158f74215de20fd5fdcee299020e8f2b88b;p=3501%2F2017steamworks

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 3e60406..918481c 100644
--- 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