update hatch code (#97)

* add new hatch mech code

* minor changes and add comments

* hardcode no delay for hatch eject

* change naming for clarity

* re-delete files from merge

* delete the imports of removed files

Author: kevinzwang

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));

Robot2019/src/main/java/frc/robot/commands/EjectHatch.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();
+    }
+  }
+}

Robot2019/src/main/java/frc/robot/commands/ToggleHatchEject.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;
   }

…java/frc/robot/commands/IntakeHatch.java …rc/robot/commands/ToggleHatchIntake.javaRobot2019/src/main/java/frc/robot/commands/IntakeHatch.java renamed to Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.java Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java renamed to Robot2019/src/main/java/frc/robot/commands/ToggleHatchIntake.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