add new hatch mech code (#86)

* add new hatch mech code

* minor changes and add comments

Author: kevinzwang

Robot2019/src/main/java/frc/robot/OI.java

@@ -14,14 +14,15 @@
 import frc.robot.commands.ActuateClimberRails;
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
-import frc.robot.commands.IntakeOnlyCargo;
+import frc.robot.commands.EjectHatch;
+import frc.robot.commands.IntakeCargo;
+import frc.robot.commands.IntakeHatch;
 import frc.robot.commands.ManualClimb;
 import frc.robot.commands.NormalDrive;
 import frc.robot.commands.ResetWobble;
 import frc.robot.commands.SetArcadeOrTank;
 import frc.robot.commands.SlowDrive;
 import frc.robot.commands.ToggleCamera;
-import frc.robot.commands.ToggleHatch;
 import frc.robot.commands.ToggleLight;
 import frc.robot.commands.WobbleDrive;
 import frc.robot.subsystems.Cargo;
@@ -40,7 +41,7 @@ public class OI {
   JoystickButton leftSlowBtn, rightSlowBtn;
   JoystickButton arcadeOrTankBtn;
   JoystickButton normDriveBtn;
-  JoystickButton toggleHatchBtn;
+  JoystickButton hatchIntakeBtn, hatchEjectBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
   JoystickButton autoClimbBtn;
@@ -68,12 +69,14 @@ public class OI {
     normDriveBtn = new JoystickButton(leftJoy, 3);
     normDriveBtn.whileHeld(new NormalDrive());
 
-    toggleHatchBtn = new JoystickButton(manipulator, Manip.X);
-    toggleHatchBtn.whenPressed(new ToggleHatch(hp));
+    hatchIntakeBtn = new JoystickButton(manipulator, Manip.X);
+    hatchIntakeBtn.whenPressed(new IntakeHatch(hp));
+    hatchEjectBtn = new JoystickButton(manipulator, Manip.Y);
+    hatchEjectBtn.whenPressed(new EjectHatch(hp));
 
-    cargoIntakeBtn = new JoystickButton(manipulator, Manip.A);
-    cargoIntakeBtn.whenPressed(new IntakeOnlyCargo(cargo, hp, dt));
-    cargoEjectBtn = new JoystickButton(manipulator, Manip.B);
+    cargoIntakeBtn = new JoystickButton(manipulator, Manip.A); // TODO: set ports to correct values
+    cargoIntakeBtn.whenPressed(new IntakeCargo(cargo));
+    cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values
     cargoEjectBtn.whenPressed(new EjectCargo(cargo));
 
     climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);

Robot2019/src/main/java/frc/robot/Robot.java

@@ -33,7 +33,7 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     dt = new Drivetrain(RobotMap.leftMaster, RobotMap.leftSlave1, RobotMap.leftSlave2, RobotMap.rightMaster,
         RobotMap.rightSlave1, RobotMap.rightSlave2, RobotMap.leftEnc, RobotMap.rightEnc, RobotMap.ahrs);
-    hp = new HatchPanel(RobotMap.hatchPistons);
+    hp = new HatchPanel(RobotMap.hatchGrabberPiston, RobotMap.hatchEjectPistons);
     cargo = new Cargo(RobotMap.cargoRoller, RobotMap.pdp, RobotMap.cargoPDPPort);
     climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons);
     lights = new Lights(RobotMap.lights);

Robot2019/src/main/java/frc/robot/RobotMap.java

@@ -37,7 +37,7 @@ public class RobotMap {
   static VictorSP climberMotor;
   static Encoder climberEncoder;
   static DoubleSolenoid climberPistons;
-  static DoubleSolenoid hatchPistons;
+  static DoubleSolenoid hatchGrabberPiston, hatchEjectPistons;
   static VictorSP cargoRoller;
   static Encoder leftEnc, rightEnc;
   static String driveMode;
@@ -70,7 +70,8 @@ public class RobotMap {
     cargoRoller = new VictorSP(0);
 
     // Initialize solenoid on hatch panel mech
-    hatchPistons = new DoubleSolenoid(7, 0); // 7 is A/Forward, 0 is B/Reverse
+    hatchGrabberPiston = new DoubleSolenoid(7, 0); // 7 is A/Forward, 0 is B/Reverse
+    hatchEjectPistons = new DoubleSolenoid(6, 1);
 
     leftEnc = new Encoder(new DigitalInput(0), new DigitalInput(1));
     rightEnc = new Encoder(new DigitalInput(2), new DigitalInput(3));

Robot2019/src/main/java/frc/robot/commands/EjectHatch.java

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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() {}
+}

…java/frc/robot/commands/ToggleHatch.java …java/frc/robot/commands/IntakeHatch.javaRobot2019/src/main/java/frc/robot/commands/ToggleHatch.java renamed to Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java Robot2019/src/main/java/frc/robot/commands/ToggleHatch.java renamed to Robot2019/src/main/java/frc/robot/commands/IntakeHatch.java

@@ -10,20 +10,27 @@
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import frc.robot.subsystems.HatchPanel;
 
-public class ToggleHatch extends InstantCommand {
+public class IntakeHatch extends InstantCommand {
   private HatchPanel hp;
 
-  public ToggleHatch(HatchPanel hp) {
-    super();
+  /**
+   * Toggles the grabbing piston of the hatch mechanism
+   */
+  public IntakeHatch(HatchPanel hp) {
     requires(hp);
-
     this.hp = hp;
   }
 
-  // Called once when the command executes
   @Override
   protected void initialize() {
-    hp.toggle();
+    switch(hp.state) {
+      case EJECTING:
+      case DEFAULT:
+        hp.grab();
+        break;
+      case GRABBING:
+        hp.reset();
+        break;
+    }
   }
-
 }

Robot2019/src/main/java/frc/robot/commands/IntakeOnlyCargo.java

@@ -12,53 +12,48 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class HatchPanel extends Subsystem {
-  private DoubleSolenoid pistons;
-  private String pistonState;
+  private DoubleSolenoid grabPiston, ejectPistons;
+  public State state;
 
   /**
    * Subsystem for controlling the hatch panel mechanism
    * 
-   * @param pistons the solenoid that controls all three pistons on the mechanism
+   * @param grabPiston the center piston that is used to grab the hatch panel through the hole
+   * @param ejectPistons the two side pistons that facilitate ejecting the hatch panel
    */
-  public HatchPanel(DoubleSolenoid pistons) {
-    this.pistons = pistons;
-    pistonState = "IN";
-    pistons.set(DoubleSolenoid.Value.kReverse);
-    SmartDashboard.putString("Hatch Piston State", pistonState);
+  public HatchPanel(DoubleSolenoid grabPiston, DoubleSolenoid ejectPistons) {
+    this.grabPiston = grabPiston;
+    this.ejectPistons = ejectPistons;
+
+    reset();
+
+    SmartDashboard.putString("Hatch Piston State", state.name());
   }
 
-  /**
-   * toggles the hatch panel pistons
-   * 
-   * @return if the pistons are extended after the call
-   */
-  public boolean toggle() {
-    if (pistons.get() == DoubleSolenoid.Value.kForward) {
-      pistons.set(DoubleSolenoid.Value.kReverse);
-      pistonState = "IN";
-      SmartDashboard.putString("Hatch Piston State", pistonState);
-      return false;
-    } else {
-      pistons.set(DoubleSolenoid.Value.kForward);
-      pistonState = "OUT";
-      SmartDashboard.putString("Hatch Piston State", pistonState);
-      return true;
-    }
+  public void reset() {
+    grabPiston.set(DoubleSolenoid.Value.kReverse);
+    ejectPistons.set(DoubleSolenoid.Value.kReverse);
+    state = State.DEFAULT;
   }
 
-  public void setIn() {
-    pistons.set(DoubleSolenoid.Value.kReverse);
-    pistonState = "IN";
-    SmartDashboard.putString("Hatch Piston State", pistonState);
+  public void grab() {
+    grabPiston.set(DoubleSolenoid.Value.kForward);
+    ejectPistons.set(DoubleSolenoid.Value.kReverse);
+    state = State.GRABBING;
   }
 
-  public void setOut() {
-    pistons.set(DoubleSolenoid.Value.kForward);
-    pistonState = "OUT";
-    SmartDashboard.putString("Hatch Piston State", pistonState);
+  public void eject() {
+    grabPiston.set(DoubleSolenoid.Value.kReverse);
+    ejectPistons.set(DoubleSolenoid.Value.kForward);
+    state = State.EJECTING;
   }
 
   @Override
-  public void initDefaultCommand() {
+  public void initDefaultCommand() {}
+
+  public enum State {
+    DEFAULT,
+    GRABBING,
+    EJECTING
   }
 }