Shooter adjustments

hammerhead226 · Aug 30, 2024 · eb43714 · eb43714
1 parent ce72e90
commit eb43714
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 346;
-  public static final String GIT_SHA = "fcf8829b06301b17107b8cc0514afcee04946385";
-  public static final String GIT_DATE = "2024-08-18 11:34:34 EDT";
-  public static final String GIT_BRANCH = "marc";
-  public static final String BUILD_DATE = "2024-08-18 15:14:30 EDT";
-  public static final long BUILD_UNIX_TIME = 1724008470026L;
+  public static final int GIT_REVISION = 347;
+  public static final String GIT_SHA = "ce72e903bd56c27c1efd826909dae699468e1732";
+  public static final String GIT_DATE = "2024-08-18 23:42:15 EDT";
+  public static final String GIT_BRANCH = "pre-goonettes";
+  public static final String BUILD_DATE = "2024-08-29 21:12:59 EDT";
+  public static final long BUILD_UNIX_TIME = 1724980379162L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/commands/AimbotTele.java b/src/main/java/frc/robot/commands/AimbotTele.java
@@ -108,21 +108,27 @@ public void angleShooter() {
     if (DriverStation.getAlliance().isPresent()) this.alliance = DriverStation.getAlliance().get();
     // Logger.recordOutput("distance to speak", Units.metersToFeet(distanceToSpeakerMeter));
     distanceToSpeakerMeter = calculateDistanceToSpeaker();
-    if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {
+    if (Units.metersToFeet(distanceToSpeakerMeter) < 6) {
+      shooter.setFlywheelRPMs(4500, 4000);
+      // } else {
+      //   shooter.setFlywheelRPMs(5700, 5400);
+      // }
+    } else if (Units.metersToFeet(distanceToSpeakerMeter) > 11.6) {
       // double shootingSpeed =
       //     MathUtil.clamp(
       //         calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter)), 3250, 5400);
       double shootingSpeed = calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter));
 
       shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed + 100);
     } else shooter.setFlywheelRPMs(5700, 5400);
+
     pivot.setPivotGoal(calculatePivotAngleDeg(distanceToSpeakerMeter));
   }
 
   private double calculateShooterSpeed(double distanceToSpeakerFeet) {
     double shooterSpeed = -986.49 * distanceToSpeakerFeet + 17294.6;
     // shooterSpeed = MathUtil.clamp(shooterSpeed, 3850, 5400);
-    shooterSpeed = MathUtil.clamp(shooterSpeed, 4000, 5400);
+    shooterSpeed = MathUtil.clamp(shooterSpeed, 4400, 5400);
     // if (distanceToSpeakerFeet >= 11) {
     // return -430.7 * distanceToSpeakerFeet + 8815;
     // } else return -600. * distanceToSpeakerFeet + 10406;
@@ -132,15 +138,21 @@ private double calculateShooterSpeed(double distanceToSpeakerFeet) {
   private double calculatePivotAngleDeg(double distanceToSpeakerMeter) {
     // pivotSetpointDeg = (-0.272 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) +
     // 60);
-    pivotSetpointDeg =
-        (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 57.68);
-    pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 34, 62);
+    // pivotSetpointDeg =
+    //     (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 57.68);
+    // pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 34, 62);
 
-    if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {
-      return 34;
+    // if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {
+    //   return 34;
+    // }
+    // Logger.recordOutput("pivot target auto", pivotSetpointDeg);
+    // return pivotSetpointDeg + 3.3;
+    pivotSetpointDeg = Units.radiansToDegrees(Math.atan(2.1 / distanceToSpeakerMeter));
+    if (Units.metersToFeet(distanceToSpeakerMeter) > 11.6) {
+      return 32;
     }
-    Logger.recordOutput("pivot target auto", pivotSetpointDeg);
-    return pivotSetpointDeg + 3.3;
+    pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 32, 62);
+    return pivotSetpointDeg;
   }
 
   private double calculateDistanceToSpeaker() {