From 0a0a0f7792fc32dbc253ddd72257ae5c047396f0 Mon Sep 17 00:00:00 2001 From: omn0mn0m Date: Sat, 11 Apr 2015 11:11:46 -0400 Subject: [PATCH] Added a manual override on the indexer --- src/org/team708/robot/OI.java | 6 ++- .../JoystickMoveClawElevator.java | 8 ++-- .../robot/commands/indexer/IndexerDown.java | 13 ++++-- .../robot/commands/indexer/IndexerUp.java | 21 +++++---- .../indexer/JoystickIndexerControl.java | 44 +++++++++++++++++++ src/org/team708/robot/subsystems/Indexer.java | 7 ++- 6 files changed, 80 insertions(+), 19 deletions(-) create mode 100644 src/org/team708/robot/commands/indexer/JoystickIndexerControl.java diff --git a/src/org/team708/robot/OI.java b/src/org/team708/robot/OI.java index bc022a5..a3147e7 100644 --- a/src/org/team708/robot/OI.java +++ b/src/org/team708/robot/OI.java @@ -42,7 +42,8 @@ public class OI { // Claw Buttons public static final int TOGGLE_CLAW_OPEN_BUTTON = Gamepad.button_R_Shoulder; // public static final int INTERRUPT_CLAW_BUTTON = Gamepad.button_L_Shoulder; - public static final int ANTI_SWAG_CLAW_BUTTON = Gamepad.button_L_Shoulder; +// public static final int ANTI_SWAG_CLAW_BUTTON = Gamepad.button_L_Shoulder; + public static final int INDEXER_MANUAL_OVERRIDE_BUTTON = Gamepad.button_L_Shoulder; // Claw Elevator Buttons public static final int clawHeightIncrementButton = Gamepad.button_B; @@ -64,7 +65,8 @@ public class OI { private static final AxisDown toteDown = new AxisDown(operatorGamepad, TOTE_AXIS); // Moves the indexer down to release the tote stack public static final Button toggleClawOpen = new JoystickButton(operatorGamepad, TOGGLE_CLAW_OPEN_BUTTON); // Opens and closes the claw on a toggle // public static final Button interruptClaw = new JoystickButton(operatorGamepad, INTERRUPT_CLAW_BUTTON); // Toggles the wrist position (horizontal/vertical) - public static final Button antiSwagClaw = new JoystickButton(operatorGamepad, ANTI_SWAG_CLAW_BUTTON); +// public static final Button antiSwagClaw = new JoystickButton(operatorGamepad, ANTI_SWAG_CLAW_BUTTON); + public static final Button indexerManualOverride = new JoystickButton(operatorGamepad, INDEXER_MANUAL_OVERRIDE_BUTTON); public static final Button clawHeightIncrement = new JoystickButton(operatorGamepad, clawHeightIncrementButton); // Increases the claw height by the height of a tote public static final Button clawHeightDecrement = new JoystickButton(operatorGamepad, clawHeightDecrementButton); // Decreases the claw height by the height of a tote public static final Button gucciGrabberToggle = new JoystickButton(operatorGamepad, GUCCI_GRABBER_TOGGLE_BUTTON); // Toggles the position of the gucci grabber diff --git a/src/org/team708/robot/commands/clawElevator/JoystickMoveClawElevator.java b/src/org/team708/robot/commands/clawElevator/JoystickMoveClawElevator.java index c07ad23..57ec0cc 100644 --- a/src/org/team708/robot/commands/clawElevator/JoystickMoveClawElevator.java +++ b/src/org/team708/robot/commands/clawElevator/JoystickMoveClawElevator.java @@ -37,11 +37,11 @@ protected void execute() { Robot.clawElevator.resetEncoder(); } else {} - if (OI.antiSwagClaw.get()) { - Robot.clawElevator.manualMove(moveSpeed * Constants.ANTISWAG); - } else { +// if (OI.antiSwagClaw.get()) { +// Robot.clawElevator.manualMove(moveSpeed * Constants.ANTISWAG); +// } else { Robot.clawElevator.manualMove(moveSpeed); - } +// } } diff --git a/src/org/team708/robot/commands/indexer/IndexerDown.java b/src/org/team708/robot/commands/indexer/IndexerDown.java index f3d96a3..2cdc13b 100644 --- a/src/org/team708/robot/commands/indexer/IndexerDown.java +++ b/src/org/team708/robot/commands/indexer/IndexerDown.java @@ -1,6 +1,7 @@ package org.team708.robot.commands.indexer; import org.team708.robot.Constants; +import org.team708.robot.OI; import org.team708.robot.Robot; import edu.wpi.first.wpilibj.command.Command; @@ -24,16 +25,20 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { -// if (!Robot.indexer.indexerDown) { - Robot.indexer.lowerIndexer(); -// } + if (!OI.indexerManualOverride.get()) { + // if (!Robot.indexer.indexerDown) { + Robot.indexer.lowerIndexer(); + // } + } else { + cancel(); + } } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { // return Math708.isWithinThreshold(Robot.toteElevator.getEncoderDistance(), -Robot.toteElevator.TOP_ENCODER_DISTANCE, threshold) // || Robot.toteElevator.elevatorDown; - return Robot.indexer.getEncoderDistance() <= -(Constants.TOTE_HEIGHT + EXTRA_TRAVEL_DISTANCE); + return Robot.indexer.getEncoderDistance() <= -(Constants.TOTE_HEIGHT + EXTRA_TRAVEL_DISTANCE) || OI.indexerManualOverride.get(); } // Called once after isFinished returns true diff --git a/src/org/team708/robot/commands/indexer/IndexerUp.java b/src/org/team708/robot/commands/indexer/IndexerUp.java index 0006daf..46c3b55 100644 --- a/src/org/team708/robot/commands/indexer/IndexerUp.java +++ b/src/org/team708/robot/commands/indexer/IndexerUp.java @@ -1,6 +1,7 @@ package org.team708.robot.commands.indexer; import org.team708.robot.Constants; +import org.team708.robot.OI; import org.team708.robot.Robot; import edu.wpi.first.wpilibj.command.Command; @@ -41,22 +42,26 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - if (Robot.indexer.toteCount == 0 && !irHasTote) { - irHasTote = (Robot.indexer.getIRDistance() <= Constants.IR_HAS_TOTE_DISTANCE); + if (!OI.indexerManualOverride.get()) { + if (Robot.indexer.toteCount == 0 && !irHasTote) { + irHasTote = (Robot.indexer.getIRDistance() <= Constants.IR_HAS_TOTE_DISTANCE); + } + + // if (!atToteLimitMax && hasToteInitially) { + Robot.indexer.raiseIndexer(); + // } + } else { + cancel(); } - -// if (!atToteLimitMax && hasToteInitially) { - Robot.indexer.raiseIndexer(); -// } } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { // return Robot.indexer.getEncoderDistance() >= Constants.TOTE_HEIGHT || !hasToteInitially; if (Robot.indexer.toteCount>=1) - return (Robot.indexer.getEncoderDistance() >= Constants.TOTE_HEIGHT) || !hasToteInitially; + return (Robot.indexer.getEncoderDistance() >= Constants.TOTE_HEIGHT) || !hasToteInitially || OI.indexerManualOverride.get(); else - return (irHasTote && (Robot.indexer.getIRDistance() > Constants.IR_HAS_TOTE_DISTANCE)) || !hasToteInitially; + return (irHasTote && (Robot.indexer.getIRDistance() > Constants.IR_HAS_TOTE_DISTANCE)) || !hasToteInitially || OI.indexerManualOverride.get(); } // Called once after isFinished returns true diff --git a/src/org/team708/robot/commands/indexer/JoystickIndexerControl.java b/src/org/team708/robot/commands/indexer/JoystickIndexerControl.java new file mode 100644 index 0000000..90d33a4 --- /dev/null +++ b/src/org/team708/robot/commands/indexer/JoystickIndexerControl.java @@ -0,0 +1,44 @@ +package org.team708.robot.commands.indexer; + +import org.team708.robot.OI; +import org.team708.robot.Robot; +import org.team708.robot.util.Gamepad; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class JoystickIndexerControl extends Command { + + public JoystickIndexerControl() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.indexer); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + if (OI.indexerManualOverride.get()) { + Robot.indexer.move(OI.operatorGamepad.getAxis(Gamepad.leftStick_Y)); + } + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/src/org/team708/robot/subsystems/Indexer.java b/src/org/team708/robot/subsystems/Indexer.java index fa2615d..48a5d13 100644 --- a/src/org/team708/robot/subsystems/Indexer.java +++ b/src/org/team708/robot/subsystems/Indexer.java @@ -2,6 +2,7 @@ import org.team708.robot.Constants; import org.team708.robot.RobotMap; +import org.team708.robot.commands.indexer.JoystickIndexerControl; import org.team708.robot.util.IRSensor; import edu.wpi.first.wpilibj.CANTalon; @@ -95,6 +96,10 @@ public void lowerIndexer() { // indexerMotorRight.set(Constants.MOTOR_REVERSE); } + public void move(double speed) { + indexerMotorLeft.set(-speed); + } + /** * Turns off the indexer so it does not move */ @@ -120,7 +125,7 @@ public void setToteCount(int toteCount) { public void initDefaultCommand() { // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); + setDefaultCommand(new JoystickIndexerControl()); } /**