cleanup

2491-NoMythic · Feb 28, 2024 · 935f6ba · 935f6ba
1 parent 7aecdbd
commit 935f6ba
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 31 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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)));