Merge branch 'main' into questnav

Author: blaze-developer

.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]
+      }
+    ]
   }

.advantagescope/Robot_Nautilus2025/model.glb

@@ -103,6 +103,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:
@@ -116,7 +119,7 @@ public Robot() throws RuntimeException {
 
         // Start AdvantageKit logger
         Logger.start();
-        
+
         // Check for valid swerve config
         var modules =
         new SwerveModuleConstants[] {

.advantagescope/Robot_Nautilus2025/model_0.glb

@@ -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;
@@ -91,6 +94,7 @@
 import frc.robot.subsystems.vision.TrackerIO;
 import frc.robot.subsystems.vision.TrackerIOQuest;
 import frc.robot.subsystems.vision.VisionConstants;
+import frc.robot.util.Mechanism3d;
 import frc.robot.util.ReefUtil;
 import frc.simulator.engine.ISimulatedSubsystem;
 
@@ -265,13 +269,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);
                     }
@@ -380,14 +384,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_;
     }