diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 5e6ab7c2..46b10bd4 100644 --- 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 index 430676e3..6c58b7c8 100644 --- 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(desiredShooterAngleShooterConstants.MAXIMUM_SHOOTER_ANGLE) { + desiredShooterAngle = ShooterConstants.MAXIMUM_SHOOTER_ANGLE; + } SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); double differenceAngle = (desiredShooterAngle - this.getShooterAngle());