(#20) Initial setup of static poses for subsystems

advantagekit_config/Robot_401_2026/config.json

@@ -0,0 +1,123 @@
+{
+  "name": "Robot 2026",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": 90
+    }
+  ],
+  "position": [
+    0.0,
+    0.0,
+    0.0
+  ],
+  "cameras": [],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0.0,
+        0.0,
+        0.0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 0
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0.0,
+        0.0,
+        0.0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 0
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0,
+        0,
+        0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 0
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0.0,
+        0.0,
+        0.0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 0
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0.0,
+        0.0,
+        0.0
+      ]
+    },
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 0
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0.0,
+        0.0,
+        0.0
+      ]
+    }
+  ]
+}

advantagekit_config/Robot_401_2026/reference.txt

@@ -0,0 +1,7 @@
+model.glb = drive
+model_0.glb = shooter base
+model_1.glb = indexer + hopper
+model_2.glb = intake
+model_3.glb = turret
+model_4.glb = climb carriage
+model_5.glb = hood

src/main/java/frc/robot/CoordinationLayer.java

@@ -1122,4 +1122,16 @@ private ShotInfo getCurrentShot(ShotInfo idealShot) {
             .orElse(idealShot.yawRadians()),
         idealShot.timeSeconds());
   }
+
+  public Optional<TurretSubsystem> getTurret() {
+    return turret;
+  }
+
+  public Optional<HoodSubsystem> getHood() {
+    return hood;
+  }
+
+  public Optional<IntakeSubsystem> getIntake() {
+    return intake;
+  }
 }

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

@@ -151,5 +151,7 @@ public void simulationInit() {}
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    robotContainer.updateRobotModel();
+  }
 }

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

@@ -7,7 +7,14 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Radians;
+
 import coppercore.metadata.CopperCoreMetadata;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -28,6 +35,7 @@
 import frc.robot.subsystems.transferroller.TransferRollerSubsystem;
 import frc.robot.subsystems.turret.TurretSubsystem;
 import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -150,6 +158,80 @@ public RobotContainer() {
     dependencyOrderedExecutor.finalizeSchedule();
   }
 
+  public void updateRobotModel() {
+    Angle turretAngle =
+        coordinationLayer
+            .getTurret()
+            .map(TurretSubsystem::getTurretAngleRobotRelative)
+            .orElse(Radians.zero());
+    Angle hoodAngle =
+        coordinationLayer.getHood().map(HoodSubsystem::getCurrentExitPitch).orElse(Radians.zero());
+    Angle intakeAngle =
+        coordinationLayer
+            .getIntake()
+            .map(IntakeSubsystem::getCurrentPivotAngle)
+            .orElse(Radians.zero());
+
+    var shooterBasePosition =
+        new Pose3d(new Translation3d(-0.058, 0.245, 0.37), new Rotation3d(0.0, 0.0, -Math.PI / 2));
+    var shooterOffsetFromReferencePoint =
+        new Transform3d(0.1045, -0.039, 0, new Rotation3d(0, 0, 0));
+    var shooterWithTurretAngle =
+        shooterBasePosition
+            .plus(shooterOffsetFromReferencePoint)
+            .plus(new Transform3d(0, 0, 0, new Rotation3d(0, 0, turretAngle.in(Radians))))
+            .plus(shooterOffsetFromReferencePoint.inverse());
+    var hoodOffsetFromShooter =
+        new Transform3d(0.1875, -0.04, 0.033, new Rotation3d(Math.PI / 2, 0, 0));
+    var hoodOffsetFromReferencePoint = new Transform3d(0, 0.021, 0.101, new Rotation3d(0, 0, 0));
+
+    var intakeOffsetFromReferencePoint =
+        new Transform3d(0.352, 0.158, 0.034, new Rotation3d(0, 0, 0));
+
+    var climbHeight = 0;
+    // The order of these components are described in
+    // advantagekit_config/Robot_401_2026/reference.txt
+    Logger.recordOutput(
+        "componentPositions",
+        new Pose3d[] {
+          // Shooter base
+          shooterWithTurretAngle,
+          // Indexer + hopper
+          new Pose3d(
+              new Translation3d(0.121, 0.025, 0.0), new Rotation3d(Math.PI / 2, 0.0, Math.PI / 2)),
+          // Intake
+          new Pose3d(
+                  new Translation3d(0.35, 0.0, 0.0), new Rotation3d(Math.PI / 2, 0.0, -Math.PI / 2))
+              .plus(intakeOffsetFromReferencePoint)
+              .plus(new Transform3d(0, 0, 0, new Rotation3d(intakeAngle.in(Radians), 0, 0)))
+              .plus(intakeOffsetFromReferencePoint.inverse()),
+          // Turret
+          new Pose3d(
+              new Translation3d(-0.099, 0.138, 0.331),
+              new Rotation3d(Math.PI / 2, 0.0, -Math.PI / 2)),
+          // Climb
+          new Pose3d(
+              new Translation3d(-0.180, -0.218, -0.190 + climbHeight),
+              new Rotation3d(Math.PI / 2, 0.0, Math.PI)),
+          // Hood
+          shooterWithTurretAngle
+              .plus(hoodOffsetFromShooter)
+              .plus(hoodOffsetFromReferencePoint)
+              .plus(
+                  new Transform3d(
+                      0,
+                      0,
+                      0,
+                      new Rotation3d(
+                          -hoodAngle.in(Radians)
+                              + Math.toRadians(
+                                  50.0), // This is an estimate of the hood offset angle
+                          0,
+                          0)))
+              .plus(hoodOffsetFromReferencePoint.inverse())
+        });
+  }
+
   /**
    * Create the auto chooser and publish it to network tables
    *

state_machine_graphs/turret.dot

@@ -34,6 +34,8 @@ digraph {
 
   WearInState -> HomingWaitForButtonState [label="When WearInState finished"];
 
+  WearInState -> HomingWaitForButtonState [label="When WearInState finished"];
+
   TrackHeadingState -> IdleState [label="Action != TrackHeading"];
 
   TestModeState -> IdleState [label="Not in turret test mode"];