X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fclimber%2FToggleWinch.java;h=0db401cd0239a8312da7905886c91a9fa008e365;hb=f74d236db406193b851bff99e4daec7b7abf35e7;hp=66c8364b1a37e78bcf993b5d4414e29d172322a7;hpb=366f1cfd9d6ad65174100145464e01b7e8d984d1;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 66c8364..0db401c 100644
--- a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
@@ -1,45 +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 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
   protected void initialize() {
+    System.out.println("toggled");
   }
 
   @Override
   protected void execute() {
-    if (driveTrain.shouldBeClimbing) {
-      driveTrain.setMotorValues(climbingSpeed, climbingSpeed);
+    if (climber.shouldBeClimbing) {
+      climber.setCANTalonsBrakeMode(climber.COAST_MODE);
+      climber.setMotorValues(climbingSpeed);
+      System.out.println("climbing");
     } else {
-      driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed);
+      climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+
+      System.out.println("braked");
+      /* Not sure if should have */
+      climber.stop();
+      end();
     }
   }
 
   @Override
   protected boolean isFinished() {
-    return Robot.getOI().toggleWinch.get();
+    return false;
   }
 
   @Override
   protected void end() {
-    driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing;
+    climber.shouldBeClimbing = !climber.shouldBeClimbing;
+    System.out.println("ended");
   }
 
   @Override