Merge pull request #59 from 2491-NoMythic/minShooterAngle

coded minimum and maximum angles for the angleShooterSubystem
2491-NoMythic · Feb 13, 2024 · 2c44e6f · 2c44e6f
2 parents 54c0ba9 + 37c3898
commit 2c44e6f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java
@@ -275,6 +275,8 @@ public static final class ShooterConstants{
   public static final double DEGREES_PER_ROTATION = 2491;
   public static final double DISTANCE_MULTIPLIER = 0;
   public static final double OFFSET_MULTIPLIER = 1;
+  public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found
+  public static final double MAXIMUM_SHOOTER_ANGLE = 120;//still has to be found
 
 
 

diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
@@ -99,6 +99,12 @@ public double calculateSpeakerAngleDifference() {
 		double desiredShooterAngle = Math
 				.toDegrees(Math.asin(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT / totalDistToSpeaker));
 		desiredShooterAngle = desiredShooterAngle+(speakerDist*DISTANCE_MULTIPLIER);
+		if(desiredShooterAngle<ShooterConstants.MINIMUM_SHOOTER_ANGLE) {
+			desiredShooterAngle = ShooterConstants.MINIMUM_SHOOTER_ANGLE;
+		}
+		if(desiredShooterAngle>ShooterConstants.MAXIMUM_SHOOTER_ANGLE) {
+			desiredShooterAngle = ShooterConstants.MAXIMUM_SHOOTER_ANGLE;
+		}
 		SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle);
 
 		double differenceAngle = (desiredShooterAngle - this.getShooterAngle());