(#20) Initial setup of static poses for subsystems

README.md

@@ -8,6 +8,7 @@ Code for our 2026 competition robot.
   - [Getting Started](#getting-started)
     - [Installing Dependencies](#installing-dependencies)
     - [Starting Simulation](#starting-simulation)
+  - [Using the Simulator](#using-the-simulator)
 
 ## Getting Started
 
@@ -38,3 +39,39 @@ Code` again. It should start up without issues. If you're still seeing
 errors, either in VSCode or from the simulate task failing, you can try
 restarting VSCode. In addition, clicking `Continue` on the warning that
 build failed is sometimes "good enough" to continue testing your code.
+
+## Using the Simulator
+### Installing dependencies
+
+- Install WPILib 2026.2.1 as described [in Zero-to-Robot](https://docs.wpilib.org/en/stable/docs/zero-to-robot/step-2/index.html).
+- Clone this repository:
+```sh
+git clone https://github.com/team401/2026-Robot-Code
+```
+
+### Setting up AdvantageScope custom assets folder
+- Launch AdvantageScope, either through:
+  - WPILib VSCode : Ctrl+Shift+P -> Start Tool -> AdvantageScope
+  - Your system app launcher/start menu
+- In the top menu bar, navigate to Help > Use Custom Assets Folder.
+  - Navigate to the 2026-Robot-Code folder and select `advantagekit_config`.
+  - **Make sure you choose `advantagekit_config` and not `Robot_401_2026`.** An easy way to do this is, when the file picker window opens to choose your custom assets folder, remain in `2026-Robot-Code`, click on `advantagekit_config`, and then press `Ok`. If you select the inner folder, the robot model won't be detected by AdvantageScope.
+
+### Launching sim and viewing robot position
+
+**If certain menus aren't visible in your Sim GUI, select `Workspace` > `Reset` in the top menu bar and your layout should be restored. Sometimes windows can be moved off screen and rendered inaccessible.**
+
+- Open the project in WPILib VSCode
+- Press Ctrl+Shift+P and type "Simulate Robot Code" to run the sim. When prompted, select "Sim GUI". This will launch Glass, the WPILib simulator application.
+- Open AdvantageScope. Open a `3D Field` tab by clicking the `+` in the top right and picking `3D Field`, or pressing `Alt+3`.
+- At the top left of the window, type `Odometry/Robot` into the search bar. Click on `NT:/AdvantageKit/RealOutputs/Odometry/Robot` from the list of options. This will your sidebar to the `Robot` field. For help navigating in AdvantageScope, see their [Navigation Docs](https://docs.advantagescope.org/getting-started/navigation).
+- Click and drag the `Robot` field from the sidebar to the bottom of the screen under **`Poses`**. Right click on the entry it creates, and select `Robot 2026` to use Typhon's CAD model.
+  - If `Robot 2026` isn't an option, wait a few seconds to make sure all models have loaded, and then make sure you've [set up your custom assets folder](#setting-up-advantagescope-custom-assets-folder) correctly.
+  - If it *still* isn't an option, please [open an issue](https://github.com/team401/2026-Robot-Code/issues/new) so we can update this README with better instructions.
+- Next, scroll in the sidebar back up to the search bar. This time, search for `componentPositions`. Select `NT:/AdvantageKit/RealOutputs/componentPositions`. Drag `componentPositions` from the sidebar **on top of** `Robot 2026` in the Poses pane at the bottom of the screen. This will tell AdvantageScope to use the list of poses logged under `componentPositions` to position the elements from the robot's 3D model.
+
+  If `componentPositions` shows up as a new robot, a ghost, or a game piece, it has been added as its own set of poses. Try dragging it from the sidebar again, but this time slightly higher, so that it is placed directly inside of the original `Robot` field. If done correctly, `componentPositions` should be indented slightly under the text of the `Robot` entry in the list.
+
+  If `componentPositions` instead shows up as a vision estimate target, it is likely already dependent on the robot, but is still not configured correctly. Click on its icon to change it to a component (should be a puzzle piece).
+- This setup will be preserved by AdvantageScope every time you reopen the app unless you close the tab.
+

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

@@ -1331,4 +1331,24 @@ 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;
+  }
+
+  public Optional<ClimberSubsystem> getClimber() {
+    return climber;
+  }
+
+  public Optional<Drive> getDrive() {
+    return drive;
+  }
 }

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

@@ -162,5 +162,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,8 +7,19 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Radians;
+
 import coppercore.metadata.CopperCoreMetadata;
 import coppercore.parameter_tools.json.JSONHandler;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+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.units.measure.Distance;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -31,6 +42,7 @@
 import frc.robot.subsystems.turret.TurretSubsystem;
 import frc.robot.util.TotalCurrentCalculator;
 import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -168,6 +180,84 @@ public void loadAutoCommands() {
     createAutoChooser(drive.orElse(null));
   }
 
+  public void updateRobotModel() {
+    var turretAngle =
+        coordinationLayer
+            .getTurret()
+            .map(TurretSubsystem::getGoalTurretHeading)
+            .orElse(Rotation2d.kZero)
+            .minus(
+                coordinationLayer.getDrive().map(Drive::getPose).orElse(Pose2d.kZero).getRotation())
+            .minus(Rotation2d.k180deg);
+    Angle hoodAngle =
+        coordinationLayer.getHood().map(HoodSubsystem::getCurrentExitPitch).orElse(Radians.zero());
+    Angle intakeAngle =
+        coordinationLayer
+            .getIntake()
+            .map(IntakeSubsystem::getCurrentPivotAngle)
+            .orElse(Radians.zero());
+    Distance climbHeight =
+        coordinationLayer.getClimber().map(ClimberSubsystem::getHeightMeters).orElse(Meters.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(turretAngle)))
+            .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));
+
+    // 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.in(Meters)),
+              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())
+        });
+  }
+
   /*
    * Process any pending HTTP requests from the tuning server
    * Calling this method in periodic ensures that the updates to the JSONConstants

src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

@@ -2,6 +2,7 @@
 
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
@@ -18,6 +19,7 @@
 import edu.wpi.first.units.VoltageUnit;
 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.units.measure.MutVoltage;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -393,4 +395,8 @@ public boolean isAtSearchPosition() {
   public boolean isAboveHangPosition() {
     return inputs.positionRadians > JsonConstants.climberConstants.hangClimbAngle.in(Radians);
   }
+
+  public Distance getHeightMeters() {
+    return Meters.of(inputs.positionRadians * 0.0145);
+  }
 }

src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.Constants;
 import frc.robot.CoordinationLayer.ShotMode;
 import frc.robot.DependencyOrderedExecutor;
 import frc.robot.DependencyOrderedExecutor.ActionKey;
@@ -184,6 +185,13 @@ public TurretSubsystem(DependencyOrderedExecutor dependencyOrderedExecutor, Moto
         .transitionTo(idleState);
 
     stateMachine.setState(homingWaitForButtonState);
+
+    // This is to prevent the turret from doing its homing sequence in sim as homing requires the
+    // physical hardstop which is absent in sim.
+    if (Constants.currentMode == Constants.Mode.SIM) {
+      stateMachine.setState(idleState);
+    }
+
     StateMachineDump.write("turret", stateMachine);
 
     // Initialize tunable numbers for test modes
@@ -465,4 +473,8 @@ public void targetGoalHeading(Rotation2d goalHeading) {
     this.requestedAction = TurretAction.TrackHeading;
     this.goalTurretHeading = goalHeading;
   }
+
+  public Rotation2d getGoalTurretHeading() {
+    return goalTurretHeading;
+  }
 }

state_machine_graphs/turret.dot

@@ -16,11 +16,11 @@ digraph {
 
   HomingWaitForMovementState ;
   HomingWaitForStoppingState ;
-  IdleState ;
+  IdleState [shape=doublecircle];
   WearInState ;
   TrackHeadingState ;
   TestModeState ;
-  HomingWaitForButtonState [shape=doublecircle];
+  HomingWaitForButtonState ;
 
   // Transitions