Add turret/auto targeting/SOTM

assets/Robot_CA26/config.json

@@ -0,0 +1,38 @@
+{
+  "name": "2026 CA26",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90.0
+    },
+    {
+      "axis": "y",
+      "degrees": 0.0
+    },
+    {
+      "axis": "z",
+      "degrees": 90.0
+    }
+  ],
+  "position": [0.0, 0.0, 0.0],
+  "cameras": [],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90.0
+        },
+        {
+          "axis": "y",
+          "degrees": 0.0
+        },
+        {
+          "axis": "z",
+          "degrees": 90.0
+        }
+      ],
+      "zeroedPosition": [0.205, 0.0, -0.375]
+    }
+  ]
+}

simgui-ds.json

@@ -94,10 +94,6 @@
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     },
-    {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
-    },
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true

src/main/java/frc/robot/Robot.java

@@ -1,16 +1,16 @@
 package frc.robot;
 
 import org.ironmaple.simulation.SimulatedArena;
-import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnField;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.util.CommandsLogging;
+import frc.robot.util.maplesim.Arena2026Rebuilt;
 
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
@@ -52,16 +52,15 @@ public void robotPeriodic() {
     CommandsLogging.logRequiredSubsystems();
 
     if (Robot.isSimulation()) {
-      Pose3d[] algaePoses = arena.getGamePiecesArrayByType("Algae");
-      Logger.recordOutput("FieldSimulation/AlgaePoses", algaePoses);
-
-      Pose3d[] coralPoses = arena.getGamePiecesArrayByType("Coral");
-      Logger.recordOutput("FieldSimulation/CoralPoses", coralPoses);
+      Pose3d[] fuelPoses = arena.getGamePiecesArrayByType("Fuel");
+      Logger.recordOutput("FieldSimulation/FuelPoses", fuelPoses);
     }
 
     Logger.recordOutput("FieldSimulation/RobotPose", m_robotContainer.getRobotPose());
     Logger.recordOutput("FieldSimulation/TargetPose",
         m_robotContainer.getSwerveDrive().field.getObject("targetPose").getPose());
+    Logger.recordOutput("FieldSimulation/AimDirection", m_robotContainer.getAimDirection());
+    Logger.recordOutput("FieldSimulation/AimTarget", new Pose3d(m_robotContainer.getAimPoint(), Rotation3d.kZero));
   }
 
   @Override
@@ -108,9 +107,15 @@ public void testPeriodic() {
 
   @Override
   public void simulationInit() {
+    // Shut down the old arena instance first to release ownership of all bodies
+    // (including the drivetrain) so they can be added to a new physics world
+    SimulatedArena.getInstance().shutDown();
+
+    SimulatedArena.overrideInstance(new Arena2026Rebuilt());
+
     arena = SimulatedArena.getInstance();
 
-    arena.addGamePiece(new ReefscapeAlgaeOnField(new Translation2d(14, 1)));
+    arena.addDriveTrainSimulation(m_robotContainer.getSwerveDrive().getMapleSimDrive().get());
   }
 
   @Override

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

@@ -7,6 +7,9 @@
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -16,24 +19,26 @@
 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;
 import frc.robot.subsystems.KickerSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
 import swervelib.SwerveDrive;
 
 public class RobotContainer {
   private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
-
+  private final TurretSubsystem turret = new TurretSubsystem();
+  private final ShooterSubsystem shooter = new ShooterSubsystem();
   private final IntakeSubsystem intake = new IntakeSubsystem();
   private final HopperSubsystem hopper = new HopperSubsystem();
   private final KickerSubsystem kicker = new KickerSubsystem();
-  private final ShooterSubsystem shooter = new ShooterSubsystem();
+  private final HoodSubsystem hood = new HoodSubsystem();
 
-  private final Superstructure superstructure = new Superstructure(shooter, null, null, intake, hopper, kicker);
+  private final Superstructure superstructure = new Superstructure(shooter, turret, hood, intake, hopper, kicker);
 
   private final SendableChooser<Command> autoChooser;
 
@@ -45,7 +50,7 @@ public RobotContainer() {
     configureBindings();
     buildNamedAutoCommands();
 
-    if (!Robot.isReal()) {
+    if (!Robot.isReal() || true) {
       DriverStation.silenceJoystickConnectionWarning(true);
     }
 
@@ -102,4 +107,18 @@ public SwerveDrive getSwerveDrive() {
   public Pose2d getRobotPose() {
     return drivebase.getPose();
   }
+
+  public Pose3d getAimDirection() {
+    // Apply robot heading first, then turret/hood rotation on top
+    Pose3d shooterPose = superstructure.getShooterPose();
+
+    var pose = drivebase.getPose3d().plus(new Transform3d(
+        shooterPose.getTranslation(), shooterPose.getRotation()));
+
+    return pose;
+  }
+
+  public Translation3d getAimPoint() {
+    return superstructure.getAimPoint();
+  }
 }

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

@@ -0,0 +1,169 @@
+package frc.robot.commands;
+
+import java.util.Map;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RPM;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Superstructure;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class ShootOnTheMoveCommand extends Command {
+  private final SwerveSubsystem drivetrain;
+  private final Superstructure superstructure;
+
+  private Supplier<Translation3d> aimPointSupplier; // The point to aim at
+  private AngularVelocity latestShootSpeed;
+  private Angle latestHoodAngle;
+  private Angle latestTurretAngle;
+
+  public ShootOnTheMoveCommand(SwerveSubsystem drivetrain, Superstructure superstructure,
+      Supplier<Translation3d> aimPointSupplier) {
+    this.drivetrain = drivetrain;
+    this.superstructure = superstructure;
+    this.aimPointSupplier = aimPointSupplier;
+
+    // We use the drivetrain to determine linear velocity, but don't require it for
+    // control. We
+    // will be using the superstructure to control the shooting mechanism so it's a
+    // requirement.
+    // addRequirements(superstructure);
+
+    // TODO: figure out if the above is actually required. Right now, when you start
+    // some other command, the auto aim can't start back up again
+  }
+
+  @Override
+  public void initialize() {
+    super.initialize();
+
+    latestHoodAngle = superstructure.getHoodAngle();
+    latestTurretAngle = superstructure.getTurretAngle();
+    latestShootSpeed = superstructure.getShooterSpeed();
+
+    // TODO: when this current command ends, we should probably cancel the dynamic
+    // aim command
+    superstructure.aimDynamicCommand(
+        () -> {
+          return this.latestShootSpeed;
+        },
+        () -> {
+          return this.latestTurretAngle;
+        },
+        () -> {
+          return this.latestHoodAngle;
+        }).schedule();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  public void execute() {
+    // Calculate trajectory to aimPoint
+    var target = aimPointSupplier.get();
+
+    var shooterLocation = drivetrain.getPose3d().getTranslation()
+        .plus(superstructure.getShooterPose().getTranslation());
+
+    // Ignore this parameter for now, the range tables will account for it :/
+    // var deltaH = target.getMeasureZ().minus(shooterLocation.getMeasureZ());
+    var shooterOnGround = new Translation2d(shooterLocation.getX(), shooterLocation.getY());
+    var targetOnGround = new Translation2d(target.getX(), target.getY());
+
+    var distanceToTarget = Meters.of(shooterOnGround.getDistance(targetOnGround));
+
+    // Get time of flight. We could try to do this analytically but for now it's
+    // easier and more realistic
+    // to use a simple linear approximation based on empirical data.
+    double timeOfFlight = getFlightTime(distanceToTarget);
+
+    // Calculate corrective vector based on our current velocity multiplied by time
+    // of flight.
+    // If we're stationary, this should be zero. If we're backing up, this will be
+    // "ahead" of the target, etc.
+    var updatedPosition = drivetrain.getFieldVelocity().times(timeOfFlight);
+    var correctiveVector = new Translation2d(updatedPosition.vxMetersPerSecond, updatedPosition.vyMetersPerSecond)
+        .unaryMinus();
+    var correctiveVector3d = new Translation3d(correctiveVector.getX(), correctiveVector.getY(), 0);
+
+    Logger.recordOutput("FieldSimulation/AimTargetCorrected",
+        new Pose3d(target.plus(correctiveVector3d), Rotation3d.kZero));
+
+    var correctedTarget = targetOnGround.plus(correctiveVector);
+
+    var vectorToTarget = drivetrain.getPose().getTranslation().minus(correctedTarget);
+
+    var correctedDistance = Meters.of(vectorToTarget.getNorm());
+    var calculatedHeading = vectorToTarget.getAngle()
+        .rotateBy(drivetrain.getHeading().unaryMinus())
+        .getMeasure();
+
+    Logger.recordOutput("ShootOnTheMove/RobotHeading", drivetrain.getHeading());
+    Logger.recordOutput("ShootOnTheMove/CalculatedHeading", calculatedHeading);
+    Logger.recordOutput("ShootOnTheMove/distanceToTarget", distanceToTarget);
+
+    latestTurretAngle = calculatedHeading;
+    latestShootSpeed = calculateRequiredShooterSpeed(correctedDistance);
+
+    // TODO: add this back if/when we have a real hood, for now, just set it to the
+    // current angle
+    // latestHoodAngle = calculateRequiredHoodAngle(correctedDistance);
+    latestHoodAngle = superstructure.getHoodAngle();
+
+    superstructure.setShooterSetpoints(
+        latestShootSpeed,
+        latestTurretAngle,
+        latestHoodAngle);
+
+    // System.out.println("Shooting at distance: " + correctedDistance + " requires
+    // speed: " + latestShootSpeed
+    // + ", hood angle: " + latestHoodAngle + ", turret angle: " +
+    // latestTurretAngle);
+  }
+
+  private double getFlightTime(Distance distanceToTarget) {
+    // Simple linear approximation based on empirical data.
+    return TIME_OF_FLIGHT_BY_DISTANCE.get(distanceToTarget.in(Meters));
+  }
+
+  private AngularVelocity calculateRequiredShooterSpeed(Distance distanceToTarget) {
+    return RPM.of(SHOOTING_SPEED_BY_DISTANCE.get(distanceToTarget.in(Meters)));
+  }
+
+  private Angle calculateRequiredHoodAngle(Distance distanceToTarget) {
+    return Degrees.of(HOOD_ANGLE_BY_DISTANCE.get(distanceToTarget.in(Meters)));
+  }
+
+  // meters, seconds
+  private static final InterpolatingDoubleTreeMap TIME_OF_FLIGHT_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries(
+      Map.entry(1.0, 1.0),
+      Map.entry(4.86, 1.5));
+
+  // meters, RPS
+  private static final InterpolatingDoubleTreeMap SHOOTING_SPEED_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries(
+      Map.entry(2.0, 2700.0),
+      Map.entry(3.0, 3000.0),
+      Map.entry(4.0, 3300.0),
+      Map.entry(4.86, 3750.0));
+
+  // meters, degrees
+  private static final InterpolatingDoubleTreeMap HOOD_ANGLE_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries(
+      Map.entry(1.0, 15.0),
+      Map.entry(2.0, 30.0),
+      Map.entry(3.0, 45.0));
+}