diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ede600e..1dbeaed9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -144,7 +144,7 @@ public RobotContainer() { //operatorController = new PS4Controller(OPERATOs_CONTROLLER_ID); PDP = new PowerDistribution(1, ModuleType.kRev); - ZeroGyroSup = driverController::getOptionsButton; + ZeroGyroSup = driverController::getPSButton; AimWhileMovingSup = driverController::getL2Button; ShootIfReadySup = driverController::getR2Button; SubwooferAngleSup = driverController::getCrossButton; diff --git a/src/main/java/frc/robot/commands/AimShooter.java b/src/main/java/frc/robot/commands/AimShooter.java index cd10d8e7..98fb8058 100644 --- a/src/main/java/frc/robot/commands/AimShooter.java +++ b/src/main/java/frc/robot/commands/AimShooter.java @@ -37,26 +37,18 @@ public void initialize() { public void execute() { if(SubwooferSupplier1.getAsBoolean()) { angleShooterSubsystem.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE); + } else if (StageAngleSupplier.getAsBoolean()) { + angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_ANGLE); + } else if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) { + angleShooterSubsystem.setDesiredShooterAngle(SmartDashboard.getNumber("amp angle", Field.AMPLIFIER_ANGLE)/*Field.AMPLIFIER_ANGLE*/); + } else if(humanPlayerSupplier.getAsBoolean()) { + angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE); + } else if(intakeSupplier.getAsBoolean()) { + angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE); } else { - if (StageAngleSupplier.getAsBoolean()) { - angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_ANGLE); - } else { - if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) { - angleShooterSubsystem.setDesiredShooterAngle(SmartDashboard.getNumber("amp angle", Field.AMPLIFIER_ANGLE)/*Field.AMPLIFIER_ANGLE*/); - } else { - if(humanPlayerSupplier.getAsBoolean()) { - angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE); - } else { - if(intakeSupplier.getAsBoolean()) { - angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE); - } else { - angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle()); - } - } - } - } - } + angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle()); } + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 82aa4312..1a4085bd 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -259,20 +259,20 @@ public static final class ShooterConstants{ public static final double DEGREES_PER_ROTATION = 360; public static final double DISTANCE_MULTIPLIER = 0.15; public static final double OFFSET_MULTIPLIER = 1; - public static final double MINIMUM_SHOOTER_ANGLE = 11.64;//still has to be found - public static final double COMP_MAXIMUM_SHOOTER_ANGLE = 108;//still has to be found - public static final double PRAC_MAXIMUM_SHOOTER_ANGLE = 101;//still has to be found - public static final double HUMAN_PLAYER_ANGLE = 97;//still has to be found - public static final double HUMAN_PLAYER_RPS = -10;//still has to be found + public static final double MINIMUM_SHOOTER_ANGLE = 11.64; + public static final double COMP_MAXIMUM_SHOOTER_ANGLE = 108; + public static final double PRAC_MAXIMUM_SHOOTER_ANGLE = 101; + public static final double HUMAN_PLAYER_ANGLE = 97; + public static final double HUMAN_PLAYER_RPS = -10; public static final double SAFE_SHOOTER_ANGLE = 15; public static final double GROUND_INTAKE_SHOOTER_ANGLE = 90; public static final double PRAC_ADJUST_EQUATION_A = 1.14168; public static final double PRAC_ADJUST_EQUATION_B = -1.22979; - public static final double COMP_ADJUST_EQUATION_A = 0.0469456;//0.0381123; - public static final double COMP_ADJUST_EQUATION_B = -0.237047;//-0.232111; - public static final double COMP_ADJUST_EQUATION_C = 0.699325;//0.938668;//dont lower too far or else it misses low - public static final double COMP_ADJUST_EQUATION_D = 1;//1.20694;//0.938668; + public static final double COMP_ADJUST_EQUATION_A = 0.0469456; + public static final double COMP_ADJUST_EQUATION_B = -0.237047; + public static final double COMP_ADJUST_EQUATION_C = 0.699325; + // public static final double COMP_ADJUST_EQUATION_D = 1; unused becuase we aren't using a cubic equation public static final double CompBotZeroOffset = 334.7; public static final double PracBotZeroOffset = 328; diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index b0e8b54f..e514a841 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -169,7 +169,7 @@ private double adjustAngleForDistance(double initialAngle, double distance) { AdjustEquationC = COMP_ADJUST_EQUATION_C; } if(Preferences.getBoolean("CompBot", false)) { - double errorMeters = /*AdjustEquationA*Math.pow(distance, 3)+*/AdjustEquationA*Math.pow(distance, 2) + AdjustEquationB*distance + AdjustEquationC; + double errorMeters = AdjustEquationA*Math.pow(distance, 2) + AdjustEquationB*distance + AdjustEquationC; return initialAngle + Math.toDegrees(Math.atan(errorMeters/distance)); } else { double errorMeters = Math.pow(AdjustEquationA, distance) + AdjustEquationB; diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index df1db2e8..849967bc 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -160,8 +160,9 @@ public DrivetrainSubsystem() { public void zeroGyroscope() { if(DriverStation.getAlliance().get() == Alliance.Red) { zeroGyroscope(180); - } - zeroGyroscope(0); + } else { + zeroGyroscope(0); + } } public void zeroGyroscope(double angleDeg) { resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(angleDeg)));