Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/RobotContainer.java

@@ -153,8 +153,8 @@ public class RobotContainer {
   public static boolean softLimits = true;
 
   // control mode switching
-  private enum ControlMode { SHOOTER, CLIMBER };
+  public static enum ControlMode { SHOOTER, CLIMBER };
-  private ControlMode currentControlMode = ControlMode.SHOOTER;
+  public static ControlMode currentControlMode = ControlMode.SHOOTER;
 
   // turret mode switching
   private enum TurretMode { MANUAL, AUTONOMOUS };
@@ -337,38 +337,8 @@ public class RobotContainer {
       .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
     
     new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
-
-    //   .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null)))
-    //   .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null)))
-    //   .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand(
-    //     new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber))))
-
-    //   .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null)))
-    //   .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand(
-    //     new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret))))
-    //   .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand(
-    //     new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood))));
-
       .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
       .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
-      // .whenReleased(EnableClimber()));
-      
-    // control turret manual mode
-    // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
-    //     .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
-    //     .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
-
-    // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
-    //   .whenPressed(new InstantCommand(() -> {
-    //     this.currentClimberMode = ClimberMode.AUTONOMOUS;
-    //   }))
-    //   .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL)))
-    //   .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL));
-
-    // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
-    //   .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS))
-    //   .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL)))
-    //   .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL));
 
     new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
     .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))

src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java

@@ -20,40 +20,42 @@ import frc4388.utility.Gains;
 public class Shoot extends CommandBase {
 
   // subsystems
-  public SwerveDrive m_swerve;
+  private SwerveDrive m_swerve;
-  public BoomBoom m_boomBoom;
+  private BoomBoom m_boomBoom;
-  public Turret m_turret;
+  private Turret m_turret;
-  public Hood m_hood;
+  private Hood m_hood;
 
   // given
-  public double m_gyroAngle;
+  private double m_gyroAngle;
-  public double m_odoX;
+  private double m_odoX;
-  public double m_odoY;
+  private double m_odoY;
-  public double m_distance;
+  private double m_distance;
 
   // targets
-  public double m_targetVel;
+  private double m_targetVel;
-  public double m_targetHood;
+  private double m_targetHood;
-  public double m_targetAngle;
+  private double m_targetAngle;
-  public Pose2d m_targetPoint;
+  private Pose2d m_targetPoint;
 
   // pid
-  public double error;
+  private double error;
-  public double prevError;
+  private double prevError;
-  public Gains gains = ShooterConstants.SHOOT_GAINS;
+  private Gains gains = ShooterConstants.SHOOT_GAINS;
-  public double kP, kI, kD;
+  private double kP, kI, kD;
-  public double proportional, integral, derivative;
+  private double proportional, integral, derivative;
-  public double time;
+  private double time;
-  public double output;
+  private double output;
-  public double normOutput;
+  private double normOutput;
-  public double tolerance;
+  private double tolerance;
-  public boolean isAimedInTolerance;
+
-  public int inverted;
+  private boolean isAimedInTolerance;
+  private int inverted;
+  private double initialSwerveRotation;
 
   // testing
-  public boolean simMode = true;
+  private boolean simMode = true;
-  public DummySensor driveDummy;
+  private DummySensor driveDummy;
-  public DummySensor turretDummy;
+  private DummySensor turretDummy;
 
   /**
    * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
@@ -117,6 +119,7 @@ public class Shoot extends CommandBase {
     m_odoY = -1;//m_swerve.getOdometry().getY();
 
     m_gyroAngle = m_swerve.getRegGyro().getDegrees();
+    initialSwerveRotation = m_gyroAngle;
 
     // get targets (shooter tables)
     m_targetVel = m_boomBoom.getVelocity(m_distance);
@@ -182,7 +185,10 @@ public class Shoot extends CommandBase {
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    // return to initial swerve rotation
+    m_swerve.driveWithInput(0, 0, initialSwerveRotation, true);
+  }
 
   // Returns true when the command should end.
   @Override