implement tank drive and toggle winch

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 840c49794dbd156ebaf131a24d9a2174a0e886aa..07b2028681b183e1b44bce9a80e5d9b4ccc7a5b5 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -31,6 +31,8 @@ public class Constants {
     public static final int DECREASE_SHOOTER_SPEED_PORT = 7;
     public static final int RESET_SHOOTER_SPEED_PORT = 5;
     public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 2;
+
+    public static final int CLIMB_PORT = 0;
   }
 
   public static class Shooter {

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index e9c055248327cc7ca4bc57741eb93fe1c997072e..7002a58bf1e4cbaeef433d5bd560c78543da1e60 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -2,6 +2,7 @@ package org.usfirst.frc.team3501.robot;
 
 import org.usfirst.frc.team3501.robot.commands.climber.BrakeCANTalons;
 import org.usfirst.frc.team3501.robot.commands.climber.CoastCANTalons;
+import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch;
 import org.usfirst.frc.team3501.robot.commands.driving.ToggleDriveGear;
 import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston;
 import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous;
@@ -41,6 +42,7 @@ public class OI {
 
   public static Button brakeCANTalons;
   public static Button coastCANTalons;
+  public static Button climb;
 
   public OI() {
     leftJoystick = new Joystick(Constants.OI.LEFT_STICK_PORT);
@@ -97,6 +99,9 @@ public class OI {
     coastCANTalons = new JoystickButton(rightJoystick,
         Constants.OI.COAST_CANTALONS_PORT);
     coastCANTalons.whenPressed(new CoastCANTalons());
+
+    climb = new JoystickButton(leftJoystick, Constants.OI.CLIMB_PORT);
+    climb.whenPressed(new ToggleWinch());
   }
 
   public static OI getOI() {

diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
new file mode 100644 (file)
index 0000000..47e464c
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team3501.robot.commands.climber;
+
+import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.Climber;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ToggleWinch extends Command {
+  Climber climber = Robot.getClimber();
+  private double climbingSpeed;
+
+  public ToggleWinch() {
+    requires(climber);
+    climbingSpeed = climber.CLIMBER_SPEED;
+  }
+
+  @Override
+  protected void initialize() {
+  }
+
+  @Override
+  protected void execute() {
+    if (climber.shouldBeClimbing) {
+      climber.setCANTalonsBrakeMode(climber.COAST_MODE);
+      climber.setMotorValues(climbingSpeed);
+    } else {
+      climber.setCANTalonsBrakeMode(climber.BRAKE_MODE);
+      end();
+    }
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  protected void end() {
+    climber.shouldBeClimbing = !climber.shouldBeClimbing;
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java
index d1e0f9c1f660f81f58e1310d668b689f146988c5..9007a545fce287951f650cad98ffa08bf9bd3515 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java
@@ -22,10 +22,13 @@ public class JoystickDrive extends Command {
 
   @Override
   protected void execute() {
-    final double thrust = OI.rightJoystick.getY();
-    final double twist = OI.rightJoystick.getTwist();
-
-    Robot.getDriveTrain().joystickDrive(-thrust, -twist);
+    // final double thrust = OI.rightJoystick.getY();
+    // final double twist = OI.rightJoystick.getTwist();
+    //
+    // Robot.getDriveTrain().joystickDrive(-thrust, -twist);
+    double left = OI.leftJoystick.getY();
+    double right = OI.rightJoystick.getY();
+    Robot.getDriveTrain().tankDrive(left, right);
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java b/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java
index 4cb6c0b8c2f4981c8424467a857cd2bb2ec9ae6e..d9268cb8f2705e921549d3f12abd2602dd51ad80 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java
@@ -2,7 +2,6 @@ package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
 import org.usfirst.frc.team3501.robot.MathLib;
-import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous;
 
 import com.ctre.CANTalon;
 
@@ -14,7 +13,8 @@ public class Climber extends Subsystem {
   public static final boolean BRAKE_MODE = true;
   public static final boolean COAST_MODE = false;
 
-  public static final double CLIMBER_SPEED = 0;
+  public static final double CLIMBER_SPEED = 1.0;
+  public boolean shouldBeClimbing = false;
 
   private CANTalon winch;
 
@@ -43,6 +43,5 @@ public class Climber extends Subsystem {
 
   @Override
   protected void initDefaultCommand() {
-    setDefaultCommand(new RunWinchContinuous());
   }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 383abd6824cdd1129bcabd1c7a22156644cd9a90..f3df881225b0e65deb11323b234fa46928246f99 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -97,6 +97,10 @@ public class DriveTrain extends Subsystem {
       robotDrive.arcadeDrive(thrust, twist, true);
   }
 
+  public void tankDrive(double left, double right) {
+    robotDrive.tankDrive(left, right);
+  }
+
   public void stop() {
     setMotorValues(0, 0);
   }