diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index 9277518..cac1ebc 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -14,9 +14,9 @@ import frc.robot.commands.ActuateClimberRails; import frc.robot.commands.Climb; import frc.robot.commands.EjectCargo; -import frc.robot.commands.EjectHatch; +import frc.robot.commands.ToggleHatchEject; import frc.robot.commands.IntakeCargo; -import frc.robot.commands.IntakeHatch; +import frc.robot.commands.ToggleHatchIntake; import frc.robot.commands.ManualClimb; import frc.robot.commands.NormalDrive; import frc.robot.commands.ResetWobble; @@ -70,9 +70,9 @@ public class OI { normDriveBtn.whileHeld(new NormalDrive()); hatchIntakeBtn = new JoystickButton(manipulator, Manip.X); - hatchIntakeBtn.whenPressed(new IntakeHatch(hp)); + hatchIntakeBtn.whenPressed(new ToggleHatchIntake(hp)); hatchEjectBtn = new JoystickButton(manipulator, Manip.Y); - hatchEjectBtn.whenPressed(new EjectHatch(hp)); + hatchEjectBtn.whenPressed(new ToggleHatchEject(hp)); cargoIntakeBtn = new JoystickButton(manipulator, Manip.A); // TODO: set ports to correct values cargoIntakeBtn.whenPressed(new IntakeCargo(cargo)); diff --git a/Robot2019/src/main/java/frc/robot/commands/EjectHatch.java b/Robot2019/src/main/java/frc/robot/commands/EjectHatch.java deleted file mode 100644 index bc664b3..0000000 --- a/Robot2019/src/main/java/frc/robot/commands/EjectHatch.java +++ /dev/null @@ -1,67 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.HatchPanel; - -public class EjectHatch extends Command { - private HatchPanel hp; - private Timer timey; - private boolean ejecting; - - /** - * Toggles the eject pistons of the hatch panel mechanism - */ - public EjectHatch(HatchPanel hp) { - requires(hp); - this.hp = hp; - - timey = new Timer(); - - if (!SmartDashboard.containsKey("Hatch Eject Delay")) { - SmartDashboard.putNumber("Hatch Eject Delay", 0.5); - } - } - - @Override - protected void initialize() { - // If the pistons are currently extend them, retract them back, otherwise release and then eject - if (hp.state == HatchPanel.State.EJECTING) { - hp.reset(); - ejecting = false; - } else { - hp.reset(); - ejecting = true; - - timey.reset(); - timey.start(); - } - } - - @Override - protected boolean isFinished() { - // if it's ejecting, wait until end of delay to extend pistons after release - return !ejecting || (timey.get() > SmartDashboard.getNumber("Hatch Eject Delay", 0.5)); - } - - @Override - protected void end() { - if (ejecting) { - hp.eject(); - } - } - - @Override - protected void execute() {} - - @Override - protected void interrupted() {} -} diff --git a/Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.java b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.java new file mode 100644 index 0000000..2acbeef --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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.InstantCommand; +import frc.robot.subsystems.HatchPanel; + +public class ToggleHatchEject extends InstantCommand { + private HatchPanel hp; + + /** + * Toggles the eject pistons of the hatch panel mechanism, also closes the + * grabbing piston when ejecting + */ + public ToggleHatchEject(HatchPanel hp) { + requires(hp); + this.hp = hp; + } + + @Override + protected void initialize() { + // If the pistons are currently extend them, retract them back, otherwise + // release and then eject + if (hp.state == HatchPanel.State.EJECTING) { + hp.reset(); + } else { + hp.reset(); + hp.eject(); + } + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java similarity index 90% rename from Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java rename to Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java index 9293dd5..67194b2 100644 --- a/Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java @@ -10,13 +10,13 @@ import edu.wpi.first.wpilibj.command.InstantCommand; import frc.robot.subsystems.HatchPanel; -public class IntakeHatch extends InstantCommand { +public class ToggleHatchIntake extends InstantCommand { private HatchPanel hp; /** * Toggles the grabbing piston of the hatch mechanism */ - public IntakeHatch(HatchPanel hp) { + public ToggleHatchIntake(HatchPanel hp) { requires(hp); this.hp = hp; } diff --git a/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java b/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java index 9d6f7fa..2bfcfc0 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/HatchPanel.java @@ -34,18 +34,21 @@ public void reset() { grabPiston.set(DoubleSolenoid.Value.kReverse); ejectPistons.set(DoubleSolenoid.Value.kReverse); state = State.DEFAULT; + SmartDashboard.putString("Hatch Piston State", state.name()); } public void grab() { grabPiston.set(DoubleSolenoid.Value.kForward); ejectPistons.set(DoubleSolenoid.Value.kReverse); state = State.GRABBING; + SmartDashboard.putString("Hatch Piston State", state.name()); } public void eject() { grabPiston.set(DoubleSolenoid.Value.kReverse); ejectPistons.set(DoubleSolenoid.Value.kForward); state = State.EJECTING; + SmartDashboard.putString("Hatch Piston State", state.name()); } @Override