diff --git a/src/main/java/frc/robot/OperatorInterface.java b/src/main/java/frc/robot/OperatorInterface.java index 5a923dc..53c86b6 100644 --- a/src/main/java/frc/robot/OperatorInterface.java +++ b/src/main/java/frc/robot/OperatorInterface.java @@ -25,6 +25,7 @@ import frc.robot.commands.FlipStop; import frc.robot.commands.HatchExtend; import frc.robot.commands.HatchRetract; +import frc.robot.commands.LoosenAndDeploy; import frc.robot.commands.NudgeLeft; import frc.robot.commands.NudgeRight; import frc.robot.commands.SquareUpCommand; @@ -187,11 +188,11 @@ protected void createCommands() { povButtonDown.whenReleased(new CancelSquareUpCommand(this.robot.getDriveSubsystem()) ); // Arms Subsystem - armsDeployButton.whenPressed(new ArmsDeploy(this.robot.getArmsSubsystem())); + armsDeployButton.whenPressed(new LoosenAndDeploy(robot.getFlipSubsystem(), robot.getArmsSubsystem())); armsSqueezeButton.whenPressed(new ArmsSqueeze(this.robot.getArmsSubsystem())); armsReleaseButton.whenPressed(new ArmsRelease(this.robot.getArmsSubsystem())); armsReverseButton.whenPressed(new ReverseDeploy(this.robot.getArmsSubsystem())); - panelArmsDeployButton.whenPressed(new ArmsDeploy(this.robot.getArmsSubsystem())); + panelArmsDeployButton.whenPressed(new LoosenAndDeploy(robot.getFlipSubsystem(), robot.getArmsSubsystem())); panelArmsSqueezeButton.whenPressed(new ArmsSqueeze(this.robot.getArmsSubsystem())); panelArmsReleaseButton.whenPressed(new ArmsRelease(this.robot.getArmsSubsystem())); diff --git a/src/main/java/frc/robot/commands/LoosenAndDeploy.java b/src/main/java/frc/robot/commands/LoosenAndDeploy.java new file mode 100644 index 0000000..56c1ea5 --- /dev/null +++ b/src/main/java/frc/robot/commands/LoosenAndDeploy.java @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.LoosenFlipRope; +import frc.robot.commands.ArmsDeploy; +import frc.robot.subsystems.ArmsSubsystem; +import frc.robot.subsystems.FlipSubsystem;; + +public class LoosenAndDeploy extends CommandGroup { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + + + + public LoosenAndDeploy(FlipSubsystem flip, ArmsSubsystem arms) { + addSequential(new LoosenFlipRope(flip)); + addSequential(new ArmsDeploy(arms)); + } +} diff --git a/src/main/java/frc/robot/commands/LoosenFlipRope.java b/src/main/java/frc/robot/commands/LoosenFlipRope.java new file mode 100644 index 0000000..b419e67 --- /dev/null +++ b/src/main/java/frc/robot/commands/LoosenFlipRope.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.subsystems.FlipSubsystem;; + +public class LoosenFlipRope extends Command { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + private FlipSubsystem flip; + private static double LOOSEN_SECONDS = 3; + + public LoosenFlipRope(FlipSubsystem flip) { + this.flip=flip; + requires(flip); + setTimeout(LOOSEN_SECONDS); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + flip.backward(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + flip.backward(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return isTimedOut(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + flip.stop(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +}