[Robot] Undo lob changes

SouthwestRoboticsProgramming · Apr 1, 2024 · 8366c1d · 8366c1d
1 parent a7e5eb8
commit 8366c1d
Showing 1 changed file with 7 additions and 7 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java
@@ -21,13 +21,14 @@ public LobCalculator() {
  }
 
  public Aim calculateAim(double distanceToSpeaker, double velocityTowardsGoal) {
- double angleRad = Math.atan2(4 * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get(), distanceToSpeaker);
+ boolean inWing = distanceToSpeaker > 5;
+ double angleRad = 0;
 
- if (Math.toDegrees(angleRad) > 65) { // If we're really close, aim for the lower arc that hits the target
- angleRad = Math.toRadians(45 - (Math.toDegrees(angleRad) - 45));
+ if (inWing) {
+ angleRad = Math.atan2(4 * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get(), distanceToSpeaker);
  }
 
- double sqrt2gh = tallSqrt2gh;
+ double sqrt2gh = (inWing) ? tallSqrt2gh : shortSqrt2gh;
 
  double velocity = sqrt2gh / Math.sin(angleRad);
 
@@ -43,13 +44,12 @@ public Aim calculateAim(double distanceToSpeaker, double velocityTowardsGoal) {
 
  @Override
  public Aim calculateAim(double distanceToSpeaker) {
- throw new RuntimeException("oh no");
-// return calculateAim(distanceToSpeaker, 0);
+ return calculateAim(distanceToSpeaker, 0);
  }
 
  private void updateHeight() {
  tallSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get());
  shortSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.get());
  }
- 
+
 }