From: Rohan Rodrigues <rohanrodrigues19@gmail.com>
Date: Wed, 8 Feb 2017 05:09:40 +0000 (-0800)
Subject: Add command group for ToggleWinch
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=25ef99e622af7d7dbd7ddb6c9c1a7ec7e71b0b77

Add command group for ToggleWinch
---

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index 32ec062..05fb72d 100644
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,7 +1,6 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commands.climber.MaintainClimbedPosition;
-import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
+import org.usfirst.frc.team3501.robot.commandgroups.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;
@@ -18,7 +17,6 @@ public class OI {
   public static Joystick leftJoystick;
   public static Joystick rightJoystick;
   public static Button toggleWinch;
-  private boolean isClimbing = false;
 
   public static Button runIndexWheel;
   public static Button reverseIndexWheel;
@@ -59,13 +57,13 @@ public class OI {
 
     toggleWinch = new JoystickButton(leftJoystick,
         Constants.OI.TOGGLE_WINCH_PORT);
-    if (!isClimbing) {
-      toggleWinch.whenPressed(new RunWinchContinuous());
-      isClimbing = true;
-    } else {
-      toggleWinch.whenPressed(new MaintainClimbedPosition());
-      isClimbing = false;
-    }
+    /*
+     * 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());
   }
 
   public static OI getOI() {
diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java
new file mode 100644
index 0000000..3766aee
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java
@@ -0,0 +1,22 @@
+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/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index f30d4c2..7463b86 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -81,6 +81,7 @@ public class DriveTrain extends Subsystem {
 
     frontRight.set(-right);
     rearRight.set(-right);
+    this.isClimbing = true;
   }
 
   public void joystickDrive(final double thrust, final double twist) {
@@ -89,6 +90,7 @@ public class DriveTrain extends Subsystem {
 
   public void stop() {
     setMotorValues(0, 0);
+    this.isClimbing = false;
   }
 
   public double getLeftMotorVal() {
@@ -192,6 +194,10 @@ public class DriveTrain extends Subsystem {
     return this.isClimbing;
   }
 
+  public void setClimbing(boolean isClimbing) {
+    this.isClimbing = isClimbing;
+  }
+
   public double getClimbingSpeed() {
     return this.CLIMBER_SPEED;
   }