add claw toggle and winch tensioning

diff --git a/src/org/usfirst/frc/team3501/robot/Joystick.java b/src/org/usfirst/frc/team3501/robot/Joystick.java
index c183c66b4b58e810c4dac12564e8509e9afa0955..f78be053bfd94622e0f11a9789b0bf24589de6ed 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Joystick.java
+++ b/src/org/usfirst/frc/team3501/robot/Joystick.java
@@ -25,4 +25,8 @@ public class Joystick extends edu.wpi.first.wpilibj.Joystick {
     public void whenReleased(int button, Command c) {
         buttons.get(button).whenReleased(c);
     }
+
+    public void whileHeld(int button, Command c) {
+        buttons.get(button).whileHeld(c);
+    }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index 58781e3b038ea24db01c3967b1d9c7f26814a941..f573281d6b4a7bb86328cfec799991cfa8826734 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -12,11 +12,21 @@ public class OI {
         right.whenPressed(1, new CloseClaw());
         right.whenReleased(1, new OpenClaw());
 
+        right.whenPressed(2, new ToggleClaw());
+
         right.whenPressed(11, new TurnOffCompressor());
         right.whenPressed(12, new TurnOffCompressor());
 
         right.whenPressed(7, new TurnOnCompressor());
         right.whenPressed(8, new TurnOnCompressor());
+
+        // potential bug point: this should "Just Work" because as soon as
+        // command stops running, left stick pos at 0 should take over...
+        // if that is not the case, arm will continue indefinitely.
+        left.whileHeld(7,  new TensionLeftWinch(RobotMap.ARM_ADJUST_SPEED));
+        left.whileHeld(6,  new TensionLeftWinch(-RobotMap.ARM_ADJUST_SPEED));
+        left.whileHeld(11, new TensionRightWinch(RobotMap.ARM_ADJUST_SPEED));
+        left.whileHeld(10, new TensionRightWinch(-RobotMap.ARM_ADJUST_SPEED));
     }
 
     public double getForwardL() {

diff --git a/src/org/usfirst/frc/team3501/robot/RobotMap.java b/src/org/usfirst/frc/team3501/robot/RobotMap.java
index 3ce51bf176aade4e46c2c6cc5647fabbb020ba1e..8cfb7fb99c177da473e8d069096e0f8690a798c6 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/RobotMap.java
+++ b/src/org/usfirst/frc/team3501/robot/RobotMap.java
@@ -23,6 +23,8 @@ public class RobotMap {
     // arm
     public static final int LEFT_WINCH_ADDRESS = 2, RIGHT_WINCH_ADDRESS = 7;
 
+    public static final double ARM_ADJUST_SPEED = 0.3;
+
     // auton
     public static final double OVER_STEP_TIME = 1.2, OVER_STEP_SPEED = 0.7,
                                PAST_STEP_TIME = 1.5, PAST_STEP_SPEED = 0.5;

diff --git a/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java b/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java
index 45bb43b36699bb0b5a08af651abf4dc633b6d206..50467045ab345c2baa5821ef041c20e67100e259 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/CommandBase.java
@@ -5,6 +5,7 @@ import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.subsystems.*;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
 
 public abstract class CommandBase extends Command {
 
@@ -28,6 +29,10 @@ public abstract class CommandBase extends Command {
         pneumatics = Robot.pneumatics;
     }
 
+    protected void schedule(Command c) {
+        Scheduler.getInstance().add(c);
+    }
+
     protected void initialize() {}
 
     protected void execute() {}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java b/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java
new file mode 100644 (file)
index 0000000..4d9712c
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/TensionLeftWinch.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class TensionLeftWinch extends CommandBase {
+
+    private double speed;
+
+    public TensionLeftWinch(double speed) {
+        super("TensionLeftWinch");
+        requires(arm);
+
+        this.speed = speed;
+    }
+
+    protected void execute() {
+        arm.moveLeft(speed);
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java b/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java
new file mode 100644 (file)
index 0000000..1bf68cf
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/TensionRightWinch.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class TensionRightWinch extends CommandBase {
+
+    private double speed;
+
+    public TensionRightWinch(double speed) {
+        super("TensionRightWinch");
+        requires(arm);
+
+        this.speed = speed;
+    }
+
+    protected void execute() {
+        arm.moveRight(speed);
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java b/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java
new file mode 100644 (file)
index 0000000..f1afa16
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/ToggleClaw.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class ToggleClaw extends CommandBase {
+
+    public ToggleClaw() {
+        super("ToggleClaw");
+        requires(claw);
+    }
+
+    protected void initialize() {
+        if (claw.isOpen())
+            schedule(new CloseClaw());
+        else
+            schedule(new OpenClaw());
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java b/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java
index 5c9ae5b6c9e3e3b7adaca6426d26c806221869d7..a4c4cb46f44fdd62a890eed4cd6589aa6307e3ff 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/Claw.java
@@ -22,5 +22,13 @@ public class Claw extends Subsystem {
         piston.set(RobotMap.CLOSED);
     }
 
+    public boolean isOpen() {
+        return piston.get().equals(RobotMap.OPEN);
+    }
+
+    public boolean isClosed() {
+        return piston.get().equals(RobotMap.CLOSED);
+    }
+
     public void initDefaultCommand() {}
 }