Add inspirational auto

Mechanical-Advantage · Apr 13, 2024 · 317fd07 · 317fd07
1 parent 8c5489c
commit 317fd07
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 23 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -461,15 +461,15 @@ private void configureButtonBindings() {
     Trigger nearSpeaker = new Trigger(robotState::inShootingZone);
     driver
         .a()
-        .and(nearSpeaker)
+        .and(nearSpeaker.or(shootPresets))
         .whileTrue(
             driveAimCommand
                 .get()
                 .alongWith(superstructureAimCommand.get(), flywheels.shootCommand())
                 .withName("Prepare Shot"));
     driver
         .a()
-        .and(nearSpeaker.negate())
+        .and(nearSpeaker.negate().and(shootPresets.negate()))
         .whileTrue(
             Commands.startEnd(
                     () ->
@@ -486,7 +486,7 @@ private void configureButtonBindings() {
     driver
         .rightTrigger()
         .and(driver.a())
-        .and(nearSpeaker)
+        .and(nearSpeaker.or(shootPresets))
         .and(readyToShoot.debounce(0.2, DebounceType.kRising))
         .onTrue(
             Commands.parallel(
@@ -498,7 +498,7 @@ private void configureButtonBindings() {
     driver
         .rightTrigger()
         .and(driver.a())
-        .and(nearSpeaker.negate())
+        .and(nearSpeaker.negate().and(shootPresets.negate()))
         .and(readyToShoot.debounce(0.3, DebounceType.kFalling))
         .onTrue(
             Commands.parallel(

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -57,7 +57,7 @@ public record AimingParameters(
   private static final LoggedTunableNumber lookahead =
       new LoggedTunableNumber("RobotState/lookaheadS", 0.35);
   private static final LoggedTunableNumber superPoopLookahead =
-      new LoggedTunableNumber("RobotState/SuperPoopLookahead", 1.0);
+      new LoggedTunableNumber("RobotState/SuperPoopLookahead", 0.1);
   private static final LoggedTunableNumber closeShootingZoneFeet =
       new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 10.0);
   private static final double poseBufferSizeSeconds = 2.0;
@@ -73,18 +73,21 @@ public record AimingParameters(
       new InterpolatingDoubleTreeMap();
 
   static {
-    superPoopArmAngleMap.put(Units.feetToMeters(30.0), 35.0);
-    superPoopArmAngleMap.put(Units.feetToMeters(25.0), 37.0);
-    superPoopArmAngleMap.put(Units.feetToMeters(22.0), 45.0);
+    superPoopArmAngleMap.put(Units.feetToMeters(33.52713263758169), 35.0);
+    superPoopArmAngleMap.put(Units.feetToMeters(28.31299227120627), 39.0);
+    superPoopArmAngleMap.put(Units.feetToMeters(25.587026383435525), 48.0);
   }
 
   private static final InterpolatingTreeMap<Double, FlywheelSpeeds> superPoopFlywheelSpeedsMap =
       new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), FlywheelSpeeds::interpolate);
 
   static {
-    superPoopFlywheelSpeedsMap.put(Units.feetToMeters(30.0), new FlywheelSpeeds(3500, 4500));
-    superPoopFlywheelSpeedsMap.put(Units.feetToMeters(25.0), new FlywheelSpeeds(4100, 4100));
-    superPoopFlywheelSpeedsMap.put(Units.feetToMeters(22.0), new FlywheelSpeeds(2700, 3700));
+    superPoopFlywheelSpeedsMap.put(
+        Units.feetToMeters(33.52713263758169), new FlywheelSpeeds(3500, 4500));
+    superPoopFlywheelSpeedsMap.put(
+        Units.feetToMeters(28.31299227120627), new FlywheelSpeeds(2800, 3500));
+    superPoopFlywheelSpeedsMap.put(
+        Units.feetToMeters(25.587026383435525), new FlywheelSpeeds(2500, 3200));
   }
 
   private static final double autoFarShotCompensationDegrees = 0.0; // 0.6 at NECMP
@@ -282,7 +285,9 @@ public AimingParameters getAimingParameters() {
   }
 
   private static final Translation2d superPoopTarget =
-      FieldConstants.Stage.podiumLeg.getTranslation().interpolate(FieldConstants.ampCenter, 0.5);
+      FieldConstants.Subwoofer.centerFace
+          .getTranslation()
+          .interpolate(FieldConstants.ampCenter, 0.5);
 
   public AimingParameters getSuperPoopAimingParameters() {
     if (latestSuperPoopParameters != null) {
@@ -293,13 +298,26 @@ public AimingParameters getSuperPoopAimingParameters() {
     Translation2d predictedRobotToTarget =
         AllianceFlipUtil.apply(superPoopTarget).minus(predictedFieldToRobot.getTranslation());
     double effectiveDistance = predictedRobotToTarget.getNorm();
+    var flywheelSpeeds = superPoopFlywheelSpeedsMap.get(effectiveDistance);
+    var armAngle = Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance));
+
+    Translation2d vehicleVelocity =
+        new Translation2d(robotVelocity.dx, robotVelocity.dy)
+            .rotateBy(predictedRobotToTarget.getAngle().unaryMinus());
+    Logger.recordOutput(
+        "RobotState/SuperPoopParameters/RadialVelocity", vehicleVelocity.getX());
+    double radialVelocity =
+        Units.radiansPerSecondToRotationsPerMinute(
+                vehicleVelocity.getX() / Units.inchesToMeters(1.5))
+            * armAngle.getCos();
+    flywheelSpeeds =
+        new FlywheelSpeeds(
+            flywheelSpeeds.leftSpeed() - radialVelocity,
+            flywheelSpeeds.rightSpeed() - radialVelocity);
 
     latestSuperPoopParameters =
         new AimingParameters(
-            predictedRobotToTarget.getAngle(),
-            Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance)),
-            effectiveDistance,
-            superPoopFlywheelSpeedsMap.get(effectiveDistance));
+            predictedRobotToTarget.getAngle(), armAngle, effectiveDistance, flywheelSpeeds);
     return latestSuperPoopParameters;
   }
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -108,6 +108,8 @@ private double getRads() {
   private TrapezoidProfile.Constraints currentConstraints = maxProfileConstraints.get();
   private TrapezoidProfile profile;
   private TrapezoidProfile.State setpointState = new TrapezoidProfile.State();
+
+  private double goalAngle;
   private ArmFeedforward ff;
 
   private final ArmVisualizer measuredVisualizer;
@@ -149,8 +151,12 @@ public void setOverrides(BooleanSupplier disableOverride, BooleanSupplier coastO
     coastSupplier = coastOverride;
   }
 
+  private boolean shouldPartialStow() {
+    return (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone());
+  }
+
   private double getStowAngle() {
-    if (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()) {
+    if (shouldPartialStow()) {
       return MathUtil.clamp(
           setpointState.position,
           minAngle.getRadians(),
@@ -202,7 +208,7 @@ public void periodic() {
     // Don't run profile when characterizing, coast mode, or disabled
     if (!characterizing && brakeModeEnabled && !disableSupplier.getAsBoolean()) {
       // Run closed loop
-      double goalAngle =
+      goalAngle =
           goal.getRads() + (goal == Goal.AIM ? Units.degreesToRadians(currentCompensation) : 0.0);
       if (goal == Goal.STOW) {
         goalAngle = getStowAngle();
@@ -217,15 +223,20 @@ public void periodic() {
                       Units.degreesToRadians(lowerLimitDegrees.get()),
                       Units.degreesToRadians(upperLimitDegrees.get())),
                   0.0));
-      io.runSetpoint(
-          setpointState.position, ff.calculate(setpointState.position, setpointState.velocity));
+      if (goal == Goal.STOW && !shouldPartialStow() && atGoal()) {
+        io.stop();
+      } else {
+        io.runSetpoint(
+            setpointState.position, ff.calculate(setpointState.position, setpointState.velocity));
+      }
+
+      goalVisualizer.update(goalAngle);
+      Logger.recordOutput("Arm/GoalAngle", goalAngle);
     }
 
     // Logs
     measuredVisualizer.update(inputs.positionRads);
     setpointVisualizer.update(setpointState.position);
-    goalVisualizer.update(goal.getRads());
-    Logger.recordOutput("Arm/GoalAngle", goal.getRads());
     Logger.recordOutput("Arm/SetpointAngle", setpointState.position);
     Logger.recordOutput("Arm/SetpointVelocity", setpointState.velocity);
     Logger.recordOutput("Superstructure/Arm/Goal", goal);
@@ -237,7 +248,7 @@ public void stop() {
 
   @AutoLogOutput(key = "Superstructure/Arm/AtGoal")
   public boolean atGoal() {
-    return EqualsUtil.epsilonEquals(setpointState.position, goal.getRads(), 1e-3);
+    return EqualsUtil.epsilonEquals(setpointState.position, goalAngle, 1e-3);
   }
 
   public void setBrakeMode(boolean enabled) {