competition fixes

[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 47e464c9f7b34ecdbd53e7e15948c9287abd9a47..0db401cd0239a8312da7905886c91a9fa008e365 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
@@ -16,6 +16,7 @@ public class ToggleWinch extends Command {
 
   @Override
   protected void initialize() {
+    System.out.println("toggled");
   }
 
   @Override
@@ -23,8 +24,13 @@ public class ToggleWinch extends Command {
     if (climber.shouldBeClimbing) {
       climber.setCANTalonsBrakeMode(climber.COAST_MODE);
       climber.setMotorValues(climbingSpeed);
+      System.out.println("climbing");
     } else {
       climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+
+      System.out.println("braked");
+      /* Not sure if should have */
+      climber.stop();
       end();
     }
   }
@@ -37,6 +43,7 @@ public class ToggleWinch extends Command {
   @Override
   protected void end() {
     climber.shouldBeClimbing = !climber.shouldBeClimbing;
+    System.out.println("ended");
   }
 
   @Override