From 30d9b49fbde45de4b162196b711bef1852ad8c69 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:29:53 -0600 Subject: [PATCH 1/4] broken button box stuff --- .../java/frc4388/robot/RobotContainer.java | 31 ++++++++++++++++--- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d3cc08..800127c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,6 +69,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; @@ -153,8 +154,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 }; @@ -359,11 +360,31 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // .whenPressed(new InstantCommand(() -> { - // this.currentClimberMode = ClimberMode.AUTONOMOUS; + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.AUTONOMOUS; } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { 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)); + + // // * custom Command inside InstantCommand + // .whenPressed(new InstantCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); } + // }) + // .until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) + + // // * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode + // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws)) + + // // * CommandChooser with BooleanSuppliers + + // // .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) + // // .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) + + // .whenReleased(new InstantCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.MANUAL; } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.MANUAL; } + // })); // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS)) From e52ba2fe4c72f50836ebce162d3e976f39b70d03 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:31:56 -0600 Subject: [PATCH 2/4] fix --- src/main/java/frc4388/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 800127c..f584997 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,7 +69,6 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; -import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; From e51f86e9c1d7e6c81e34965878e24e2366c42723 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:33:22 -0600 Subject: [PATCH 3/4] fixed merge conflict before it even happens lol --- .../java/frc4388/robot/RobotContainer.java | 50 ------------------- 1 file changed, 50 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f584997..68ea963 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -337,58 +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(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.AUTONOMOUS; } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.AUTONOMOUS; } - // })) - - // // * custom Command inside InstantCommand - // .whenPressed(new InstantCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); } - // }) - // .until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - - // // * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode - // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws)) - - // // * CommandChooser with BooleanSuppliers - - // // .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - // // .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) - - // .whenReleased(new InstantCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.MANUAL; } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { 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)) From e19dc2d77c25436ef6999ee83519a636e2d699fd Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 20:02:23 -0600 Subject: [PATCH 4/4] private > public, return to swerve rotation --- .../robot/commands/ShooterCommands/Shoot.java | 60 ++++++++++--------- 1 file changed, 33 insertions(+), 27 deletions(-) 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