From 731aef6d7e789d087a0ae0462ad45cca17ac7bc1 Mon Sep 17 00:00:00 2001
From: Rohan Rodrigues <rohanrodrigues19@gmail.com>
Date: Tue, 7 Feb 2017 21:09:40 -0800
Subject: [PATCH] Add command group for ToggleWinch

---
 src/org/usfirst/frc/team3501/robot/OI.java    |  8 ++++++-
 .../robot/commandgroups/ToggleWinch.java      | 22 +++++++++++++++++++
 .../team3501/robot/subsystems/DriveTrain.java |  6 +++++
 3 files changed, 35 insertions(+), 1 deletion(-)
 create mode 100644 src/org/usfirst/frc/team3501/robot/commandgroups/ToggleWinch.java

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index 8b5cdac..40df56d 100644
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -21,7 +21,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;
@@ -83,6 +82,13 @@ public class OI {
       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 2a4ccf7..0718539 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -118,6 +118,7 @@ public class DriveTrain extends Subsystem {
 
     frontRight.set(-right);
     rearRight.set(-right);
+    this.isClimbing = true;
   }
 
   public void joystickDrive(final double thrust, final double twist) {
@@ -126,6 +127,7 @@ public class DriveTrain extends Subsystem {
 
   public void stop() {
     setMotorValues(0, 0);
+    this.isClimbing = false;
   }
 
   public double getLeftMotorVal() {
@@ -229,6 +231,10 @@ public class DriveTrain extends Subsystem {
     return this.isClimbing;
   }
 
+  public void setClimbing(boolean isClimbing) {
+    this.isClimbing = isClimbing;
+  }
+
   public double getClimbingSpeed() {
     return this.CLIMBER_SPEED;
   }
-- 
2.30.2