Align to Amp and RobotContainer Cleanup

Remapped align to source to align to the amp, removed excess comments from robotcontainer

Author: Jon-Zimmerman

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 = 347;
-  public static final String GIT_SHA = "ce72e903bd56c27c1efd826909dae699468e1732";
-  public static final String GIT_DATE = "2024-08-18 23:42:15 EDT";
+  public static final int GIT_REVISION = 348;
+  public static final String GIT_SHA = "eb437146e720e43f1e7b134ae90f118a354d22da";
+  public static final String GIT_DATE = "2024-08-29 21:18:58 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 String BUILD_DATE = "2024-09-05 14:52:13 EDT";
+  public static final long BUILD_UNIX_TIME = 1725562333869L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/frc/robot/RobotContainer.java

@@ -56,7 +56,7 @@ public AimbotTele(
 
     switch (Constants.currentMode) {
       case REAL:
-        gains[0] = 3.7;
+        gains[0] = 3.2;
         gains[1] = 0;
         gains[2] = 0;
         break;
@@ -113,22 +113,22 @@ public void angleShooter() {
       // } else {
       //   shooter.setFlywheelRPMs(5700, 5400);
       // }
-    } else if (Units.metersToFeet(distanceToSpeakerMeter) > 11.6) {
+    } else if (Units.metersToFeet(distanceToSpeakerMeter) > 12.5) {
       // 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);
+      shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed + 400);
+    } else shooter.setFlywheelRPMs(5700, 5000);
 
     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, 4400, 5400);
+    shooterSpeed = MathUtil.clamp(shooterSpeed, 4400, 5300);
     // if (distanceToSpeakerFeet >= 11) {
     // return -430.7 * distanceToSpeakerFeet + 8815;
     // } else return -600. * distanceToSpeakerFeet + 10406;
@@ -147,8 +147,8 @@ private double calculatePivotAngleDeg(double distanceToSpeakerMeter) {
     // }
     // Logger.recordOutput("pivot target auto", pivotSetpointDeg);
     // return pivotSetpointDeg + 3.3;
-    pivotSetpointDeg = Units.radiansToDegrees(Math.atan(2.1 / distanceToSpeakerMeter));
-    if (Units.metersToFeet(distanceToSpeakerMeter) > 11.6) {
+    pivotSetpointDeg = Units.radiansToDegrees(Math.atan(2.1 / distanceToSpeakerMeter)) + 0.5;
+    if (Units.metersToFeet(distanceToSpeakerMeter) > 12.5) {
       return 32;
     }
     pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 32, 62);

src/main/java/frc/robot/commands/AimbotTele.java

@@ -132,10 +132,10 @@ public static Command joystickDrive(
       DoubleSupplier ySupplier,
       DoubleSupplier omegaSupplier,
       BooleanSupplier intakeAssistSupplier,
-      BooleanSupplier turnToSourceSupplier) {
+      BooleanSupplier turnToAmpSupplier) {
     return Commands.run(
         () -> {
-          rotationPID.setTolerance(5);
+          rotationPID.setTolerance(1);
           rotationPID.enableContinuousInput(-180, 180);
           sidewaysPID.setTolerance(0.05460);
           // Apply deadband
@@ -178,14 +178,15 @@ public static Command joystickDrive(
 
           sideWaysError = 0 - drive.getNotePositionRobotRelative().getY();
 
-          if (turnToSourceSupplier.getAsBoolean()) {
+          if (turnToAmpSupplier.getAsBoolean()) {
             Rotation2d curreRotation2d = drive.getRotation();
             Rotation2d targeRotation2d;
-            if (DriverStation.getAlliance().get() == Alliance.Blue) {
-              targeRotation2d = Rotation2d.fromDegrees(-60);
-            } else {
-              targeRotation2d = Rotation2d.fromDegrees(240);
-            }
+            // if (DriverStation.getAlliance().get() == Alliance.Blue) {
+            //   targeRotation2d = Rotation2d.fromDegrees(-60); //-60 for blue source
+            // } else {
+            //   targeRotation2d = Rotation2d.fromDegrees(240); //240 for red source
+            // }
+            targeRotation2d = Rotation2d.fromDegrees(90); // TODO 90 or -90 for amp, need to test
             rotationPID.setSetpoint(targeRotation2d.getDegrees());
 
             wantedRotationVelocity =

src/main/java/frc/robot/commands/DriveCommands.java

@@ -63,15 +63,15 @@ public TurnToAmpCorner(
     }
 
     pid = new PIDController(gains[0], gains[1], gains[2], 0.02);
-    pid.setTolerance(4);
+    pid.setTolerance(2);
     pid.enableContinuousInput(-180, 180);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pivot.setPivotGoal(45);
-    shooter.setFlywheelRPMs(4800, 4200);
+    pivot.setPivotGoal(50);
+    shooter.setFlywheelRPMs(3700, 2700);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/TurnToAmpCorner.java

@@ -196,7 +196,7 @@ public NoteState seesNote() {
       lastNoteState = NoteState.SENSOR;
       return NoteState.SENSOR;
 
-    } else if (feedInputs.currentAmps > 13) {
+    } else if (feedInputs.currentAmps > 13) { //TODO add additional check to filter out false positives
       // } else if (feedInputs.currentAmps > 10000) {
       Logger.recordOutput("see note val", "current");
       lastNoteState = NoteState.CURRENT;

src/main/java/frc/robot/subsystems/Shooter/Shooter.java

@@ -208,8 +208,6 @@ public void periodic() {
       module.periodic();
     }
 
-    Logger.recordOutput("test trans", new Translation2d(2, 4));
-
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {
       for (var module : modules) {