MORE PEW PEW

AdvantageScopeSwerve.json

@@ -70,8 +70,6 @@
             "/AdvantageKit",
             "/AdvantageKit/RealOutputs",
             "/AdvantageKit/RealOutputs/ShootOnTheMove/RobotHeading",
-            "/AdvantageKit/RealOutputs/ASCalibration",
-            "/AdvantageKit/RealOutputs/FieldSimulation",
             "/AdvantageKit/RealOutputs/FieldSimulation/Shooter"
           ]
         },
@@ -111,7 +109,7 @@
                     "type": "ghost",
                     "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/TargetPose",
                     "logType": "Pose2d",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "model": "Crab Bot",
                       "color": "#0000ff"
@@ -131,7 +129,7 @@
                     "type": "ghost",
                     "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/AimTarget",
                     "logType": "Pose3d",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "model": "Duck Bot",
                       "color": "#00ffff"
@@ -141,7 +139,7 @@
                     "type": "ghost",
                     "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/AimTargetCorrected",
                     "logType": "Pose3d",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "model": "Duck Bot",
                       "color": "#ff00ff"
@@ -185,17 +183,17 @@
                 "cameraIndex": -1,
                 "orbitFov": 50,
                 "cameraPosition": [
-                  10.378995714785386,
-                  9.101397756194608,
-                  -0.10752311567010889
+                  11.534767787918012,
+                  9.794331756031207,
+                  -5.627252825256971
                 ],
                 "cameraTarget": [
-                  4.495231078701326,
-                  -1.3207821976493825,
-                  -0.22073703807687897
+                  6.837520944122143,
+                  2.222662928789005,
+                  -1.7627162173818105
                 ]
               },
-              "controlsHeight": -570
+              "controlsHeight": 519
             },
             {
               "type": 1,

src/main/java/frc/robot/Constants.java

@@ -2,10 +2,39 @@
 
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 import swervelib.math.Matter;
 
 public final class Constants {
 
+  public static enum AimPoints {
+    RED_HUB(new Translation3d(11.938, 4.034536, 1.5748)),
+    RED_OUTPOST(new Translation3d(15.75, 7.25, 0)),
+    RED_FAR_SIDE(new Translation3d(15.75, 0.75, 0)),
+
+    BLUE_HUB(new Translation3d(4.5974, 4.034536, 1.5748)),
+    BLUE_OUTPOST(new Translation3d(0.75, 0.75, 0)),
+    BLUE_FAR_SIDE(new Translation3d(0.75, 7.25, 0));
+
+    public final Translation3d value;
+
+    private AimPoints(Translation3d value) {
+      this.value = value;
+    }
+
+    public static final Translation3d getAllianceHubPosition() {
+      return DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? RED_HUB.value : BLUE_HUB.value;
+    }
+
+    public static final Translation3d getAllianceOutpostPosition() {
+      return DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? RED_OUTPOST.value : BLUE_OUTPOST.value;
+    }
+
+    public static final Translation3d getAllianceFarSidePosition() {
+      return DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? RED_FAR_SIDE.value : BLUE_FAR_SIDE.value;
+    }
+  }
+
   public static final double ROBOT_MASS = Units.lbsToKilograms(120); // 32lbs * kg per pound
   public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
   public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms spark max velocity lag

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

@@ -10,15 +10,20 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import static edu.wpi.first.units.Units.Inches;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.ControllerConstants;
 import frc.robot.controls.DriverControls;
 import frc.robot.controls.OperatorControls;
+import frc.robot.controls.PoseControls;
 import frc.robot.subsystems.HoodSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -42,6 +47,9 @@ public class RobotContainer {
 
   private final SendableChooser<Command> autoChooser;
 
+  // Track current alliance for change detection
+  private Alliance currentAlliance = Alliance.Red;
+
   /**
    * The container for the robot. Contains subsystems, I/O devices, and commands.
    */
@@ -50,6 +58,20 @@ public RobotContainer() {
     configureBindings();
     buildNamedAutoCommands();
 
+    // Initialize alliance (default to red if not present)
+    onAllianceChanged(getAlliance());
+
+    // Set up trigger to detect alliance changes
+    new Trigger(() -> getAlliance() != currentAlliance)
+        .onTrue(Commands.runOnce(() -> onAllianceChanged(getAlliance())).ignoringDisable(true));
+
+    // Triggers for auto aim/pass poses
+    new Trigger(() -> isInAllianceZone())
+        .onChange(Commands.runOnce(() -> onZoneChanged()).ignoringDisable(true));
+
+    new Trigger(() -> isOnAllianceOutpostSide())
+        .onChange(Commands.runOnce(() -> onZoneChanged()).ignoringDisable(true));
+
     if (!Robot.isReal() || true) {
       DriverStation.silenceJoystickConnectionWarning(true);
     }
@@ -72,7 +94,7 @@ private void configureBindings() {
     // Set up controllers
     DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, superstructure);
     OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, superstructure);
-    // PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase);
+    PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase);
   }
 
   private void buildNamedAutoCommands() {
@@ -121,4 +143,64 @@ public Pose3d getAimDirection() {
   public Translation3d getAimPoint() {
     return superstructure.getAimPoint();
   }
+
+  public void setAimPoint(Translation3d aimPoint) {
+    superstructure.setAimPoint(aimPoint);
+  }
+
+  private Alliance getAlliance() {
+    return DriverStation.getAlliance().orElse(Alliance.Red);
+  }
+
+  private boolean isInAllianceZone() {
+    Alliance alliance = getAlliance();
+    Distance blueZone = Inches.of(182);
+    Distance redZone = Inches.of(469);
+
+    if (alliance == Alliance.Blue && drivebase.getPose().getMeasureX().lt(blueZone)) {
+      return true;
+    } else if (alliance == Alliance.Red && drivebase.getPose().getMeasureX().gt(redZone)) {
+      return true;
+    }
+
+    return false;
+  }
+
+  private boolean isOnAllianceOutpostSide() {
+    Alliance alliance = getAlliance();
+    Distance midLine = Inches.of(158.84375);
+
+    if (alliance == Alliance.Blue && drivebase.getPose().getMeasureY().lt(midLine)) {
+      return true;
+    } else if (alliance == Alliance.Red && drivebase.getPose().getMeasureY().gt(midLine)) {
+      return true;
+    }
+
+    return false;
+  }
+
+  private void onZoneChanged() {
+    if (isInAllianceZone()) {
+      superstructure.setAimPoint(Constants.AimPoints.getAllianceHubPosition());
+    } else {
+      if (isOnAllianceOutpostSide()) {
+        superstructure.setAimPoint(Constants.AimPoints.getAllianceOutpostPosition());
+      } else {
+        superstructure.setAimPoint(Constants.AimPoints.getAllianceFarSidePosition());
+      }
+    }
+  }
+
+  private void onAllianceChanged(Alliance alliance) {
+    currentAlliance = alliance;
+
+    // Update aim point based on alliance
+    if (alliance == Alliance.Blue) {
+      superstructure.setAimPoint(Constants.AimPoints.BLUE_HUB.value);
+    } else {
+      superstructure.setAimPoint(Constants.AimPoints.RED_HUB.value);
+    }
+
+    System.out.println("Alliance changed to: " + alliance);
+  }
 }

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

@@ -153,6 +153,10 @@ private Angle calculateRequiredHoodAngle(Distance distanceToTarget) {
   private static final InterpolatingDoubleTreeMap TIME_OF_FLIGHT_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries(
       Map.entry(1.0, 1.0),
       Map.entry(4.86, 1.5));
+  // TODO: add more data points here.
+  // CLOSE: NEED
+  // MID: maybe good enough
+  // FAR: NEED
 
   // meters, RPS
   private static final InterpolatingDoubleTreeMap SHOOTING_SPEED_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries(

src/main/java/frc/robot/controls/DriverControls.java

@@ -8,7 +8,6 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import static edu.wpi.first.units.Units.Feet;
 import static edu.wpi.first.units.Units.Meter;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -109,19 +108,15 @@ public static Command fireFuel(SwerveSubsystem drivetrain, Superstructure supers
     return Commands.runOnce(() -> {
       SimulatedArena arena = SimulatedArena.getInstance();
 
-      System.out.println("FIRE!");
-
       GamePieceProjectile fuel = new RebuiltFuelOnFly(
           drivetrain.getPose().getTranslation(),
           new Translation2d(
               superstructure.turret.turretTranslation.getX() * -1,
               superstructure.turret.turretTranslation.getY()),
           drivetrain.getSwerveDrive().getRobotVelocity(),
-          superstructure.getAimRotation3d().toRotation2d(),
-          Feet.of(superstructure.turret.turretTranslation.getZ()),
+          drivetrain.getPose().getRotation().rotateBy(superstructure.getAimRotation3d().toRotation2d()),
+          superstructure.turret.turretTranslation.getMeasureZ(),
 
-          // based on numbers from https://www.reca.lc/flywheel
-          // Adjust for simulation tuning
           // 0.5 times because we're applying spin to the fuel as we shoot it
           superstructure.getTangentialVelocity().times(0.5),
           superstructure.getHoodAngle());

src/main/java/frc/robot/controls/OperatorControls.java

@@ -87,45 +87,4 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur
             .ignoringDisable(true)
             .withName("OperatorControls.aimCommand"));
   }
-
-  public static Command fireAlgae(SwerveSubsystem drivetrain, Superstructure superstructure) {
-    return Commands.runOnce(() -> {
-      System.err.println("FIRE!");
-
-      SimulatedArena arena = SimulatedArena.getInstance();
-
-      // Translation2d robotPosition,
-      // Translation2d shooterPositionOnRobot,
-      // ChassisSpeeds chassisSpeeds,
-      // Rotation2d shooterFacing,
-      // Distance initialHeight,
-      // LinearVelocity launchingSpeed,
-      // Angle shooterAngle
-
-      GamePieceProjectile fuel = new RebuiltFuelOnFly(
-          drivetrain.getPose().getTranslation(),
-          new Translation2d(),
-          drivetrain.getSwerveDrive().getRobotVelocity().times(-1),
-          superstructure.getAimRotation3d().toRotation2d(),
-          Distance.ofBaseUnits(1, Feet),
-
-          // based on numbers from https://www.reca.lc/flywheel
-          // superstructure.getTangentialVelocity().times(0.5), // adjust for simulation
-          // tuning
-          LinearVelocity.ofBaseUnits(5, FeetPerSecond),
-          superstructure.getHoodAngle());
-
-      // Configure callbacks to visualize the flight trajectory of the projectile
-      fuel.withProjectileTrajectoryDisplayCallBack(
-          // Callback for when the note will eventually hit the target (if configured)
-          (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot",
-              pose3ds.toArray(Pose3d[]::new)),
-          // Callback for when the note will eventually miss the target, or if no target
-          // is configured
-          (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot",
-              pose3ds.toArray(Pose3d[]::new)));
-
-      arena.addGamePieceProjectile(fuel);
-    }).withName("Fire.Fuel");
-  }
 }

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants;
 
 /**
  * Superstructure coordinates the shooter, turret, hood, and intake subsystems
@@ -35,17 +36,17 @@ public class Superstructure extends SubsystemBase {
   private static final Angle HOOD_TOLERANCE = Degrees.of(2);
 
   // Triggers for readiness checks
-  public final Trigger isShooterAtSpeed;
-  public final Trigger isTurretOnTarget;
-  public final Trigger isHoodOnTarget;
-  public final Trigger isReadyToShoot;
+  private final Trigger isShooterAtSpeed;
+  private final Trigger isTurretOnTarget;
+  private final Trigger isHoodOnTarget;
+  private final Trigger isReadyToShoot;
 
   private AngularVelocity targetShooterSpeed = RPM.of(0);
   private Angle targetTurretAngle = Degrees.of(0);
   private Angle targetHoodAngle = Degrees.of(0);
 
-  // Hard coded red hub aim point
-  private Translation3d aimPoint = new Translation3d(Meter.of(11.902), Meter.of(4.031), Meter.of(0));
+  // Default aim point is red hub
+  private Translation3d aimPoint = Constants.AimPoints.RED_HUB.value;
 
   public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake,
       HopperSubsystem hopper, KickerSubsystem kicker) {

src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java

@@ -1,9 +1,12 @@
 package frc.robot.util.maplesim;
 
 import org.ironmaple.simulation.gamepieces.GamePieceProjectile;
+import org.ironmaple.utils.FieldMirroringUtils;
+import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Meters;
@@ -48,6 +51,18 @@ public RebuiltFuelOnFly(
                 shooterAngle);
 
         super.withTouchGroundHeight(Inches.of(3).in(Meters));
+
+        super.withTargetPosition(
+                () -> FieldMirroringUtils.toCurrentAllianceTranslation(new Translation3d(11.938, 4.034536, 1.5748)));
+
+        Logger.recordOutput("HubGoal", new Translation3d(11.938, 4.034536, 1.5748));
+
+        super.withTargetTolerance(
+                new Translation3d(
+                        Inches.of(23.5).in(Meters),
+                        Inches.of(23.5).in(Meters),
+                        Inches.of(1).in(Meters)));
+
         super.enableBecomesGamePieceOnFieldAfterTouchGround();
     }
 }

src/main/java/frc/robot/util/maplesim/RebuiltHub.java

@@ -92,6 +92,11 @@ protected boolean checkCollision(GamePiece GamePiece) {
         + Math.pow(GamePiece.getPose3d().getZ() - position.getZ(), 2) < Math.pow(GoalRadius, 2);
   }
 
+  @Override
+  protected boolean checkRotation(GamePiece GamePiece) {
+    return true;
+  }
+
   @Override
   protected void addPoints() {
     arena.addValueToMatchBreakdown(isBlue, "TotalFuelInHub", 1);