diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3273411..e5f83c6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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 }; - private ControlMode currentControlMode = ControlMode.SHOOTER; + public static enum ControlMode { SHOOTER, CLIMBER }; + 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)) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index aabac8f..5f4ae4c 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/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; - public BoomBoom m_boomBoom; - public Turret m_turret; - public Hood m_hood; + private SwerveDrive m_swerve; + private BoomBoom m_boomBoom; + private Turret m_turret; + private Hood m_hood; // given - public double m_gyroAngle; - public double m_odoX; - public double m_odoY; - public double m_distance; + private double m_gyroAngle; + private double m_odoX; + private double m_odoY; + private double m_distance; // targets - public double m_targetVel; - public double m_targetHood; - public double m_targetAngle; - public Pose2d m_targetPoint; + private double m_targetVel; + private double m_targetHood; + private double m_targetAngle; + private Pose2d m_targetPoint; // pid - public double error; - public double prevError; - public Gains gains = ShooterConstants.SHOOT_GAINS; - public double kP, kI, kD; - public double proportional, integral, derivative; - public double time; - public double output; - public double normOutput; - public double tolerance; - public boolean isAimedInTolerance; - public int inverted; + private double error; + private double prevError; + private Gains gains = ShooterConstants.SHOOT_GAINS; + private double kP, kI, kD; + private double proportional, integral, derivative; + private double time; + private double output; + private double normOutput; + private double tolerance; + + private boolean isAimedInTolerance; + private int inverted; + private double initialSwerveRotation; // testing - public boolean simMode = true; - public DummySensor driveDummy; - public DummySensor turretDummy; + private boolean simMode = true; + private DummySensor driveDummy; + 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