feat: Physically accurate robot dimensions, gamepiece and intake simulation

src/main/deploy/swerve/maxSwerve/modules/backleft.json

@@ -21,7 +21,7 @@
   "absoluteEncoderInverted": true,
   "absoluteEncoderOffset": 301.3301969,
   "location": {
-    "front": -12,
-    "left": 12
+    "front": -16,
+    "left": 14
   }
 }

src/main/deploy/swerve/maxSwerve/modules/backright.json

@@ -21,7 +21,7 @@
   "absoluteEncoderInverted": true,
   "absoluteEncoderOffset": 7.6941204,
   "location": {
-    "front": -12,
-    "left": -12
+    "front": -16,
+    "left": -14
   }
 }

src/main/deploy/swerve/maxSwerve/modules/frontleft.json

@@ -21,7 +21,7 @@
   "absoluteEncoderInverted": true,
   "absoluteEncoderOffset": 90,
   "location": {
-    "front": 12,
-    "left": 12
+    "front": 16,
+    "left": 14
   }
 }

src/main/deploy/swerve/maxSwerve/modules/frontright.json

@@ -21,7 +21,7 @@
   "absoluteEncoderInverted": true,
   "absoluteEncoderOffset": -90,
   "location": {
-    "front": 12,
-    "left": -12
+    "front": 16,
+    "left": -14
   }
 }

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

@@ -4,11 +4,15 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.ironmaple.simulation.SimulatedArena;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -21,6 +25,13 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
+  StructArrayPublisher<Pose3d> coralPoses =
+      NetworkTableInstance.getDefault().getStructArrayTopic("Coral", Pose3d.struct).publish();
+  StructArrayPublisher<Pose3d> algaePoses =
+      NetworkTableInstance.getDefault().getStructArrayTopic("Algae", Pose3d.struct).publish();
+
+  private static Robot m_instance;
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -31,13 +42,19 @@ public void robotInit() {
     // and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    m_instance = this;
 
     if (isSimulation()) {
       DriverStation.refreshData();
       DriverStation.silenceJoystickConnectionWarning(true);
+      SimulatedArena.getInstance().resetFieldForAuto();
     }
   }
 
+  public static Robot getInstance() {
+    return m_instance;
+  }
+
   /**
    * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
    * that you want ran during disabled, autonomous, teleoperated and test.
@@ -87,7 +104,7 @@ public void autonomousInit() {
   @Override
   public void autonomousPeriodic() {
     // SmartDashboard.putString(
-    //     "Current Pose Auto", RobotContainer.m_driveSubsystem.getPose().toString());
+    // "Current Pose Auto", RobotContainer.m_driveSubsystem.getPose().toString());
   }
 
   @Override
@@ -114,4 +131,12 @@ public void testInit() {
   /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {}
+
+  @Override
+  public void simulationPeriodic() {
+    coralPoses.accept(
+        SimulatedArena.getInstance().getGamePiecesByType("Coral").toArray(Pose3d[]::new));
+    algaePoses.accept(
+        SimulatedArena.getInstance().getGamePiecesByType("Algae").toArray(Pose3d[]::new));
+  }
 }

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.CoralHandlerSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import java.io.File;
 import swervelib.SwerveInputStream;
@@ -20,23 +21,25 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  private final SwerveSubsystem m_drive =
-      new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/maxSwerve"));
+  private final SwerveSubsystem m_drive = new SwerveSubsystem(
+      new File(Filesystem.getDeployDirectory(), "swerve/maxSwerve"));
+  private final CoralHandlerSubsystem m_coral = new CoralHandlerSubsystem(m_drive.getSimDrive());
 
   private final CommandXboxController m_driverController = new CommandXboxController(0);
 
   // Configure drive input stream
-  SwerveInputStream driveInput =
-      SwerveInputStream.of(
-              m_drive.getSwerveDrive(),
-              () -> m_driverController.getLeftY(),
-              () -> m_driverController.getLeftX())
-          .withControllerRotationAxis(() -> -m_driverController.getRightX())
-          .deadband(0.1)
-          .scaleTranslation(0.8)
-          .allianceRelativeControl(true);
+  SwerveInputStream driveInput = SwerveInputStream.of(
+      m_drive.getSwerveDrive(),
+      () -> m_driverController.getLeftY(),
+      () -> m_driverController.getLeftX())
+      .withControllerRotationAxis(() -> -m_driverController.getRightX())
+      .deadband(0.1)
+      .scaleTranslation(0.8)
+      .allianceRelativeControl(true);
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
     configureButtonBindings();
 
@@ -45,19 +48,25 @@ public RobotContainer() {
   }
 
   /**
-   * Use this method to define your button->command mappings. Buttons can be created by
-   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * Use this method to define your button->command mappings. Buttons can be
+   * created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its
+   * subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling
+   * passing it to a
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
     // Zero gyro with A button
     m_driverController.a().onTrue(Commands.runOnce(m_drive::zeroGyro));
 
     // Lock wheels with left bumper
+    m_driverController
+        .rightBumper()
+        .onTrue(Commands.runOnce(m_coral::intake));
     m_driverController
         .leftBumper()
-        .whileTrue(Commands.runOnce(m_drive::lock, m_drive).repeatedly());
+        .onTrue(Commands.runOnce(m_coral::outtake));
   }
 
   /**

src/main/java/frc/robot/subsystems/CoralHandlerSubsystem.java

@@ -1,5 +1,101 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.Rev2mDistanceSensor;
+import com.revrobotics.Rev2mDistanceSensor.Unit;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
+import org.dyn4j.geometry.Polygon;
+import org.dyn4j.geometry.Rectangle;
+import org.dyn4j.geometry.Vector2;
+import org.ironmaple.simulation.IntakeSimulation;
+import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
+
+public class CoralHandlerSubsystem extends SubsystemBase {
+    // two neo 550s, 9:1 gearing on both, going in opposite directions.
+    // looking on from the top, the right one going clockwise is intake, and
+    // counterclockwise is
+    // outtake.
+    // vice versa for the other
+    // TODO: move all of the hardcoded values to Constants
+
+    public final SparkMax m_left;
+    public final SparkMax m_right;
+    private final Rev2mDistanceSensor m_sensor;
+
+    public final IntakeSimulation m_intakeSim;
+
+    public enum Mode {
+        kInactive,
+        kIntake,
+        kOuttake,
+    }
+
+    private Mode m_mode = Mode.kInactive;
+    private boolean m_hasCoral = false;
+
+    public CoralHandlerSubsystem(AbstractDriveTrainSimulation driveSim) {
+        m_left = new SparkMax(9, MotorType.kBrushless);
+        m_right = new SparkMax(10, MotorType.kBrushless);
+        if (Robot.getInstance().isReal()) {
+            m_sensor = new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kOnboard);
+        } else {
+            m_sensor = null;
+        }
+        m_intakeSim = new IntakeSimulation(
+                "Coral",
+                driveSim,
+                new Rectangle(.762, 1.007),
+                1);
+    }
+
+    public Mode getMode() {
+        return m_mode;
+    }
+
+    public boolean hasCoral() {
+        return m_hasCoral;
+    }
+
+    public void setMode(Mode mode) {
+        m_mode = mode;
+        switch (mode) {
+            case kInactive:
+                m_left.set(0);
+                m_right.set(0);
+                break;
+            case kIntake:
+                m_left.set(1);
+                m_right.set(-1);
+                break;
+            case kOuttake:
+                m_left.set(-1);
+                m_right.set(1);
+                break;
+        }
+    }
+
+    @Override
+    public void periodic() {
+        if (Robot.getInstance().isReal()) {
+            m_hasCoral = m_sensor.getRange(Unit.kInches) < 5;
+        } else {
+            if (m_mode == Mode.kIntake) {
+                m_intakeSim.startIntake();
+            } else {
+                m_intakeSim.stopIntake();
+            }
+            m_hasCoral = m_intakeSim.getGamePiecesAmount() != 0;
+        }
+    }
+
+    public void intake() {
+        setMode(Mode.kIntake);
+    }
 
-public class CoralHandlerSubsystem extends SubsystemBase {}
+    public void outtake() {
+        setMode(Mode.kOuttake);
+    }
+}