Update image, articulate 3D model for AdvantageScope to render, simulation fixes, smoother simulation.

.advantagescope/Robot_Nautilus2025/config.json

@@ -1,6 +1,5 @@
 {
     "name": "Nautilus",
-    "disableSimplification": false,
     "rotations": [
       {"axis": "x", "degrees": 90}
     ],
@@ -15,15 +14,15 @@
           }
         ],
         "position": [
-          0.1975,
-          -0.2032,
-          0.205
+          0.3048,
+          0.12,
+          0.12
         ],
         "resolution": [
           1280,
           960
         ],
-        "fov": 82
+        "fov": 83
       },
       {
         "name": "Back Camera",
@@ -38,9 +37,9 @@
           }
         ],
         "position": [
-          -0.3159252,
-          -0.0508,
-          0.1760474
+          -0.3048,
+          0,
+          0.12
         ],
         "resolution": [
           960,
@@ -53,17 +52,40 @@
         "rotations": [
           {
             "axis": "y",
-            "degrees": -20
+            "degrees": -25
           },
           {
             "axis": "z",
             "degrees": 100
           }
         ],
         "position": [
-          0.077343,
-          -0.322961,
-          0.1825752
+          -0.12,
+          0.3048,
+          0.12
+        ],
+        "resolution": [
+          960,
+          720
+        ],
+        "fov": 62.5
+      },
+      {
+        "name": "Right Camera",
+        "rotations": [
+          {
+            "axis": "y",
+            "degrees": -20
+          },
+          {
+            "axis": "z",
+            "degrees": -90
+          }
+        ],
+        "position": [
+          0.07,
+          -0.3048,
+          0.50
         ],
         "resolution": [
           960,
@@ -72,5 +94,22 @@
         "fov": 62.5
       }
     ],
-    "components": []
+    "components": [
+      {
+        "zeroedRotations": [{"axis": "x", "degrees": 90}],
+        "zeroedPosition": [0, 0, 0]
+      },
+      {
+        "zeroedRotations": [{"axis": "x", "degrees": 90}],
+        "zeroedPosition": [0, 0, -0.06]
+      },
+      {
+        "zeroedRotations": [{"axis": "x", "degrees": 90}],
+        "zeroedPosition": [-0.28575, 0, -0.48]
+      },
+      {
+        "zeroedRotations": [{"axis": "x", "degrees": 90}],
+        "zeroedPosition": [0.124, -0.298, -0.450]
+      }
+    ]
   }

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

@@ -102,6 +102,9 @@ public Robot() throws RuntimeException {
             case SIM:
                 // Running a physics simulator, log to NT
                 Logger.addDataReceiver(new NT4Publisher());
+
+                // Silence Joystick Warnings
+                DriverStation.silenceJoystickConnectionWarning(RobotBase.isSimulation());
                 break;
 
             case REPLAY:
@@ -115,7 +118,7 @@ public Robot() throws RuntimeException {
 
         // Start AdvantageKit logger
         Logger.start();
-        
+
         // Check for valid swerve config
         var modules =
         new SwerveModuleConstants[] {

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

@@ -42,6 +42,7 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.Mode;
 import frc.robot.Constants.ReefLevel;
+import frc.robot.Constants.RobotType;
 import frc.robot.commands.auto.AutoCommands;
 import frc.robot.commands.auto.AutoModeBaseCmd;
 import frc.robot.commands.drive.DriveCommands;
@@ -61,6 +62,7 @@
 import frc.robot.subsystems.brain.SetLevelCmd;
 import frc.robot.subsystems.climber.ClimberIO;
 import frc.robot.subsystems.climber.ClimberIOHardware;
+import frc.robot.subsystems.climber.ClimberIOSim;
 import frc.robot.subsystems.climber.ClimberSubsystem;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
@@ -73,6 +75,7 @@
 import frc.robot.subsystems.funnel.FunnelSubsystem;
 import frc.robot.subsystems.grabber.GrabberIO;
 import frc.robot.subsystems.grabber.GrabberIOHardware;
+import frc.robot.subsystems.grabber.GrabberIOSim;
 import frc.robot.subsystems.grabber.GrabberSubsystem;
 import frc.robot.subsystems.manipulator.ManipulatorConstants;
 import frc.robot.subsystems.manipulator.ManipulatorIO;
@@ -89,6 +92,7 @@
 import frc.robot.subsystems.vision.CameraIOLimelight4;
 import frc.robot.subsystems.vision.CameraIOPhotonSim;
 import frc.robot.subsystems.vision.VisionConstants;
+import frc.robot.util.Mechanism3d;
 import frc.robot.util.ReefUtil;
 import frc.simulator.engine.ISimulatedSubsystem;
 
@@ -268,13 +272,13 @@ private RobotContainer() {
                     }
 
                     try {
-                        grabber_ = new GrabberSubsystem(new GrabberIOHardware());
+                        grabber_ = new GrabberSubsystem(new GrabberIOSim(drivebase_::getPose));
                     } catch (Exception ex) {
                         subsystemCreateException(ex);
                     }
 
                     try {
-                        climber_ = new ClimberSubsystem(new ClimberIOHardware());
+                        climber_ = new ClimberSubsystem(new ClimberIOSim());
                     } catch (Exception ex) {
                         subsystemCreateException(ex);
                     }
@@ -379,14 +383,35 @@ private RobotContainer() {
         autonomousTab.add("Auto Mode", autoChooser_.getSendableChooser()).withSize(2, 1);
         tuningTab.add("Tuning Modes", tuningChooser_.getSendableChooser()).withSize(2, 1);
 
+        // Visualizers
+        new Mechanism3d(
+            "Measured",
+            drivebase_::getPose,
+            manipulator_::getElevatorPosition,
+            manipulator_::getArmPosition,
+            climber_::getClimberPosition,
+            brain_::gp
+        );
+
+        new Mechanism3d(
+            "Setpoints",
+            drivebase_::getPose,
+            manipulator_::getElevatorTarget,
+            manipulator_::getArmTarget,
+            climber_::getClimberPosition,
+            brain_::gp
+        );
+
         // Configure the button bindings
         configureDriveBindings();
         configureButtonBindings();
         configureTestModeBindings() ;
 
-        manipulator_.setDefaultCommand(new CalibrateCmd(manipulator_));
+        if (Constants.getRobot() != RobotType.SIMBOT) {
+            manipulator_.setDefaultCommand(new CalibrateCmd(manipulator_));
+        }
     }
-
+    
     public Drive drivebase() {
         return drivebase_;
     }

src/main/java/frc/robot/commands/robot/placecoral/PlaceCoralCmd.java

@@ -42,7 +42,7 @@
 
 public class PlaceCoralCmd extends XeroSequenceCmd {
 
-    private static final boolean kUseFastL4Place = false ;
+    private static final boolean kUseFastL4Place = true;
     private static final Distance kRaiseElevatorDistance = Centimeters.of(150.0) ;
 
     private final Drive drive_;

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

@@ -3,11 +3,13 @@
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.units.measure.Time;
 
 public class ClimberConstants {
@@ -40,6 +42,9 @@ public class Climber{
         // gear ratio
         public static final double kGearRatio = 480.0 ;
 
+        // MOI used for simulation
+        public static final MomentOfInertia kMOI = KilogramSquareMeters.of(0.1);
+
         // if motor is inverted 
         public static final boolean kInverted = false; 
         public static final Current kcurrentLimit = Amps.of(80);
@@ -50,6 +55,8 @@ public class Climber{
         public static final Angle kMaxClimberAngle = Degrees.of(100);
         public static final Angle kMinClimberAngle = Degrees.of(-63) ;
 
+        public static final Angle kStartAbsEncoderAngle = Degrees.zero();
+
         public class PID {
             public static final double kP = 2.0; 
             public static final double kI = 0.0 ;
@@ -75,5 +82,6 @@ public class Position {
             public static final Angle kClimbed = Degrees.of(-58.0);
             public static final Angle kReapplyOffset = Degrees.of(0.5);
         }
+
     }   
 }

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

@@ -21,8 +21,8 @@ public class ClimberIOHardware implements ClimberIO {
 
     private static int kSyncWaitCount = 20 ;
 
-    private TalonFX climber_motor_;
-    private DutyCycleEncoder encoder_; 
+    protected TalonFX climber_motor_;
+    protected DutyCycleEncoder encoder_; 
     private int sync_count_ ;
 
     private StatusSignal<Angle> climber_pos_sig_; 
@@ -118,11 +118,21 @@ public void setClimberVoltage(Voltage voltage) {
         climber_motor_.setControl(new VoltageOut(voltage)) ;
     }
 
-    private double encoderToRobot(double encoderValue) {
+    protected double encoderToRobot(double encoderValue) {
         if (encoderValue < ClimberConstants.kMinAbsEncoderRollover) {
             encoderValue += 1.0 ;
         }
 
         return encoderValue * ClimberConstants.kMConvertAbsToRobot + ClimberConstants.kBConvertAbsToRobot;
     }
+
+    protected double robotToEncoder(double robotValue) {
+        double encoderValue = (robotValue - ClimberConstants.kBConvertAbsToRobot) / ClimberConstants.kMConvertAbsToRobot;
+
+        if (encoderValue >= ClimberConstants.kMinAbsEncoderRollover + 1.0) {
+            encoderValue -= 1.0;
+        }
+
+        return encoderValue;
+    }
 }

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

@@ -0,0 +1,54 @@
+package frc.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.sim.TalonFXSimState;
+
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.DutyCycleEncoderSim;
+import frc.robot.Robot;
+
+public class ClimberIOSim extends ClimberIOHardware {
+    private final DCMotorSim climberSim;
+    private final DutyCycleEncoderSim encoderSim;
+
+    public ClimberIOSim() throws Exception {
+        LinearSystem<N2, N1, N2> sys = LinearSystemId.createDCMotorSystem(
+            DCMotor.getKrakenX60Foc(1), 
+            ClimberConstants.Climber.kMOI.in(KilogramSquareMeters), 
+            ClimberConstants.Climber.kGearRatio
+        );
+
+        climberSim = new DCMotorSim(sys, DCMotor.getKrakenX60Foc(1));
+
+        encoderSim = new DutyCycleEncoderSim(encoder_);
+        encoderSim.set(robotToEncoder(ClimberConstants.Climber.kStartAbsEncoderAngle.in(Degrees)));
+    }
+
+    @Override
+    public void updateInputs(ClimberIOInputsAutoLogged inputs) {
+        TalonFXSimState st = climber_motor_.getSimState();
+        st.setSupplyVoltage(RobotController.getBatteryVoltage());
+
+        Voltage mv = st.getMotorVoltageMeasure();
+        climberSim.setInputVoltage(mv.in(Volts));
+        climberSim.update(Robot.defaultPeriodSecs);
+
+        st.setRawRotorPosition(climberSim.getAngularPosition().times(ClimberConstants.Climber.kGearRatio));
+        st.setRotorVelocity(climberSim.getAngularVelocity().times(ClimberConstants.Climber.kGearRatio));
+
+        super.updateInputs(inputs);
+        inputs.absEncoderRawValue = encoderSim.get();
+        inputs.absEncoderValue = Degrees.of(encoderToRobot(encoderSim.get()));
+    }
+
+}