GrabberSim

Author: blaze-developer

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

@@ -56,7 +56,6 @@
 import frc.robot.generated.PracticeTunerConstants;
 import frc.robot.subsystems.brain.BrainSubsystem;
 import frc.robot.subsystems.brain.ExecuteRobotActionCmd;
-import frc.robot.subsystems.brain.GamePiece;
 import frc.robot.subsystems.brain.QueueRobotActionCmd;
 import frc.robot.subsystems.brain.RobotAction;
 import frc.robot.subsystems.brain.SetCoralSideCmd;
@@ -76,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;
@@ -272,7 +272,7 @@ private RobotContainer() {
                     }
 
                     try {
-                        grabber_ = new GrabberSubsystem(new GrabberIOHardware());
+                        grabber_ = new GrabberSubsystem(new GrabberIOSim(drivebase_::getPose));
                     } catch (Exception ex) {
                         subsystemCreateException(ex);
                     }
@@ -390,8 +390,7 @@ private RobotContainer() {
             manipulator_::getElevatorPosition,
             manipulator_::getArmPosition,
             climber_::getClimberPosition,
-            () -> brain_.gp() == GamePiece.ALGAE_HIGH,
-            () -> brain_.gp() == GamePiece.CORAL
+            brain_::gp
         );
 
         new Mechanism3d(
@@ -400,8 +399,7 @@ private RobotContainer() {
             manipulator_::getElevatorTarget,
             manipulator_::getArmTarget,
             climber_::getClimberPosition,
-            () -> brain_.gp() == GamePiece.ALGAE_HIGH,
-            () -> brain_.gp() == GamePiece.CORAL
+            brain_::gp
         );
 
         // Configure the button bindings

src/main/java/frc/robot/subsystems/grabber/GrabberIOSim.java

@@ -0,0 +1,51 @@
+package frc.robot.subsystems.grabber;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import java.util.List;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.units.measure.Distance;
+import frc.robot.Constants;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.RobotType;
+
+public class GrabberIOSim extends GrabberIOHardware {
+
+    private static final Distance collectTolerance = Meters.one();
+
+    private final Supplier<Pose2d> robotPose_;
+    private final List<Pose2d> stations_;
+
+    // 1, 2, 13, 12
+    
+    public GrabberIOSim(Supplier<Pose2d> robotPose) throws Exception {
+        robotPose_ = robotPose;
+
+        stations_ = List.of(
+            tagPose(1),
+            tagPose(2),
+            tagPose(12),
+            tagPose(13)
+        );
+    }
+
+    @Override
+    public void updateInputs(GrabberIOInputs inputs) {
+        super.updateInputs(inputs);
+
+        if (Constants.getRobot() != RobotType.XEROSIM) {
+            Pose2d robot = robotPose_.get();
+            Pose2d nearestStation = robot.nearest(stations_);
+
+            inputs.coralSensor = robot.getTranslation().getDistance(nearestStation.getTranslation()) <= collectTolerance.in(Meters);
+            inputs.algaeSensor1 = true;
+            inputs.algaeSensor2 = true;
+        }
+    }
+
+    private Pose2d tagPose(int id) {
+        return FieldConstants.layout.getTagPose(id).orElseThrow().toPose2d();
+    }
+}

src/main/java/frc/robot/util/Mechanism3d.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.brain.GamePiece;
 
 public class Mechanism3d extends SubsystemBase {
 
@@ -40,8 +41,7 @@ public class Mechanism3d extends SubsystemBase {
     private final Supplier<Distance> elevatorHeightSupplier_;
     private final Supplier<Angle> armAngleSupplier_;
     private final Supplier<Angle> climberAngleSupplier_;
-    private final Supplier<Boolean> hasAlgaeSupplier_;
-    private final Supplier<Boolean> hasCoralSupplier_;
+    private final Supplier<GamePiece> gpSupplier_;
 
     private Pose2d robotPose_ = new Pose2d();
     private Distance elevatorHeight_ = Meters.zero();
@@ -54,16 +54,14 @@ public Mechanism3d(
         Supplier<Distance> elevatorHeight,
         Supplier<Angle> armAngle,
         Supplier<Angle> climberAngle,
-        Supplier<Boolean> hasAlgae,
-        Supplier<Boolean> hasCoral
+        Supplier<GamePiece> gamePiece
     ) {
         key_ = key;
         robotPoseSupplier_ = robotPose;
         elevatorHeightSupplier_ = elevatorHeight;
         armAngleSupplier_ = armAngle;
         climberAngleSupplier_ = climberAngle;
-        hasAlgaeSupplier_ = hasAlgae;
-        hasCoralSupplier_ = hasCoral;
+        gpSupplier_ = gamePiece;
     }
 
     @Override
@@ -90,11 +88,11 @@ public void periodic() {
             new Rotation3d(climberAngle_.unaryMinus(), Degrees.zero(), Degrees.zero())
         );
 
-        Logger.recordOutput("Gamepiece3d/Algae/" + key_, hasAlgaeSupplier_.get() ?
+        Logger.recordOutput("Gamepiece3d/Algae/" + key_, gpSupplier_.get() == GamePiece.ALGAE_HIGH ?
             new Pose3d[] { robotToField(arm.plus(armToAlgae)) } : new Pose3d[0]
         );
 
-        Logger.recordOutput("Gamepiece3d/Coral/" + key_, hasCoralSupplier_.get() ?
+        Logger.recordOutput("Gamepiece3d/Coral/" + key_, gpSupplier_.get() == GamePiece.CORAL ?
             new Pose3d[] { robotToField(arm.transformBy(armToCoral)) } : new Pose3d[0]
         );