Merge pull request #20 from 2491-NoMythic/fixedShooterMath

Fixed shooter math
2491-NoMythic · Jan 25, 2024 · f2c1d6a · f2c1d6a
2 parents ebe1ca5 + a8e019b
commit f2c1d6a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java
@@ -273,7 +273,6 @@ public static final class ShooterConstants{
   public static final double SHOOTER_ANGLE_TOLERANCE = 0.5;
   public static final double SHOOTER_HEIGHT = 0.1;
   public static final double ANGLE_TICKS_PER_DEGREE = 2491;
-  public static final double SHOOTING_SPEED_MPS = 2491;
   public static final double DEGREES_PER_ROTATION = 2491;
 
 

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -41,8 +41,8 @@ public class ShooterSubsystem extends SubsystemBase {
   double differenceAngle;
 	 double currentHeading;
 	 double speakerDist;
-	 double speakerA;
-	 double speakerB;
+	 double deltaY;
+	 double deltaX;
 	 double m_DesiredShooterAngle;
 
    SparkPIDController shooterPID;
@@ -168,12 +168,12 @@ public double calculateSpeakerAngle() {
 		//triangle for robot angle
 		Optional<Alliance> alliance = DriverStation.getAlliance();
 		if (alliance.isPresent() && alliance.get() == Alliance.Red) {
-			speakerA = Math.abs(dtvalues.getX() - Field.RED_SPEAKER_X);
+			deltaY = Math.abs(dtvalues.getY() - Field.RED_SPEAKER_Y);
 		} else {
-			speakerA = Math.abs(dtvalues.getX() - Field.BLUE_SPEAKER_X);
+			deltaY = Math.abs(dtvalues.getY() - Field.BLUE_SPEAKER_Y);
 		}
-		speakerB = Math.abs(dtvalues.getY() - Field.SPEAKER_Y);
-		speakerDist = Math.sqrt(Math.pow(speakerA, 2) + Math.pow(speakerB, 2));
+		deltaX = Math.abs(dtvalues.getX() - Field.SPEAKER_X);
+		speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2));
 		SmartDashboard.putNumber("dist to speakre", speakerDist);
 
 		shootingTime = speakerDist/shootingSpeed; //calculates how long the note will take to reach the target
@@ -183,8 +183,8 @@ public double calculateSpeakerAngle() {
 		//line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime
 
 		//next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed
-		offsetSpeakerX = speakerA-targetOffset.getX();
-		offsetSpeakerY = speakerB-targetOffset.getY();
+		offsetSpeakerX = deltaX-targetOffset.getX();
+		offsetSpeakerY = deltaY-targetOffset.getY();
 		offsetSpeakerdist = Math.sqrt(Math.pow(offsetSpeakerX, 2) + Math.pow(offsetSpeakerY, 2));
 		SmartDashboard.putString("offset amount", targetOffset.toString());
 		SmartDashboard.putString("offset speaker location", new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());