feat(swerve): Implement simulated swerve.

simgui-ds.json

@@ -90,10 +90,12 @@
     }
   ],
   "robotJoysticks": [
-    {},
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true
+    },
+    {
+      "useGamepad": true
     }
   ]
 }

simgui.json

@@ -6,6 +6,15 @@
   },
   "NetworkTables": {
     "transitory": {
+      "Shuffleboard": {
+        "Swerve": {
+          "North East Module": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
+      },
       "shooter": {
         "beamBreakSensorSim": {
           "open": true

src/main/java/frc/lib/CAN.java

@@ -2,7 +2,7 @@
 
 import java.util.Objects;
 
-/** Record class representing a CAN identifier for a device. */
+/** A CAN identifier for a device. */
 public record CAN(int id, String bus) {
 
   /**

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

@@ -52,6 +52,9 @@ private void initializeTelemetry() {
 
   /** Configures operator controller bindings. */
   private void configureBindings() {
+    driver.a().whileTrue(swerve.forwards());
+    driver.b().whileTrue(swerve.sideways());
+
     operator.leftBumper().whileTrue(shooter.intake());
     operator.leftTrigger().whileTrue(shooter.smartIntake());
 

src/main/java/frc/robot/swerve/AzimuthEncoderIO.java

@@ -0,0 +1,19 @@
+package frc.robot.swerve;
+
+/** Azimuth encoder hardware interface. */
+public interface AzimuthEncoderIO {
+
+    /** Values for the azimuth encoder hardware interface. */
+    public static class AzimuthEncoderIOValues {
+        /** Position of the azimuth encoder in rotations.  */
+        public double angleRotations = 0.0;
+    }
+
+    /**
+     * Updates the azimuth encoder's values.
+     *
+     * @param values
+     */
+    public void update(AzimuthEncoderIOValues values);
+
+}

src/main/java/frc/robot/swerve/AzimuthEncoderIOSim.java

@@ -0,0 +1,11 @@
+package frc.robot.swerve;
+
+/** Simulated azimuth encoder. */
+public class AzimuthEncoderIOSim implements AzimuthEncoderIO {
+
+    @Override
+    public void update(AzimuthEncoderIOValues values) {
+        values.angleRotations = 0.0;
+    }
+
+}

src/main/java/frc/robot/swerve/DriveMotorIO.java

@@ -0,0 +1,35 @@
+package frc.robot.swerve;
+
+/** Drive motor hardware interface. */
+public interface DriveMotorIO {
+
+    /** Values for the drive motor hardware interface. */
+    public static class DriveMotorIOValues {
+        /** Position of the drive motor in meters. */
+        public double positionMeters = 0.0;
+
+        /** Velocity of the drive motor in meters per second. */
+        public double velocityMetersPerSecond = 0.0;
+    }
+
+    /**
+     * Updates the drive motor's values.
+     *
+     * @param values
+     */
+    public void update(DriveMotorIOValues values);
+
+    /**
+     * Sets the drive motor's position.
+     * 
+     * @param positionMeters the drive motor's position.
+     */
+    public void setPosition(double positionMeters);
+
+    /**
+     * Sets the drive motor's setpoint.
+     * 
+     * @param velocityMetersPerSecond the drive motor's setpoint.
+     */
+    public void setSetpoint(double velocityMetersPerSecond);
+}

src/main/java/frc/robot/swerve/DriveMotorIOSim.java

@@ -0,0 +1,32 @@
+package frc.robot.swerve;
+
+import frc.robot.RobotConstants;
+
+/** Simulated drive motor. */
+public class DriveMotorIOSim implements DriveMotorIO {
+
+    /** Represents the position of the drive motor. */
+    private double positionMeters; 
+
+    /** Represents the velocity of the drive motor. */
+    private double velocityMetersPerSecond;
+
+    @Override
+    public void update(DriveMotorIOValues values) {
+        this.positionMeters += velocityMetersPerSecond * RobotConstants.TICK_PERIOD;
+
+        values.positionMeters = positionMeters;
+        values.velocityMetersPerSecond = velocityMetersPerSecond;
+    }
+
+    @Override
+    public void setPosition(double positionMeters) {
+        this.positionMeters = positionMeters;
+    }
+
+    @Override
+    public void setSetpoint(double velocityMetersPerSecond) {
+        this.velocityMetersPerSecond = velocityMetersPerSecond;
+    } 
+
+}

src/main/java/frc/robot/swerve/SteerMotorIO.java

@@ -0,0 +1,35 @@
+package frc.robot.swerve;
+
+/** Steer motor hardware interface. */
+public interface SteerMotorIO {
+
+    /** Values for the steer motor hardware interface. */
+    public static class SteerMotorIOValues {
+        /** Position of the steer motor in rotations.  */
+        public double angleRotations = 0.0;
+        /** Velocity of the steer motor in rotations per second.  */
+        public double omegaRotationsPerSecond = 0.0;
+    }
+
+    /**
+     * Updates the steer motor's values.
+     *
+     * @param values
+     */
+    public void update(SteerMotorIOValues values);
+
+    /**
+     * Sets the steer motor's position.
+     * 
+     * @param angleRotations the steer motor's position.
+     */
+    public void setPosition(double angleRotations);
+
+    /**
+     * Sets the steer motor's setpoint.
+     * 
+     * @param angleRotations the steer motor's setpoint.
+     */
+    public void setSetpoint(double angleRotations);
+
+}

src/main/java/frc/robot/swerve/SteerMotorIOSim.java

@@ -0,0 +1,56 @@
+package frc.robot.swerve;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import frc.robot.RobotConstants;
+import frc.robot.swerve.SwerveConstants.MK4iConstants;
+
+/** Simulated steer motor. */
+public class SteerMotorIOSim implements SteerMotorIO {
+
+    /** Represents the motor used to steer the wheel. */
+    private final DCMotor motorSim = DCMotor.getFalcon500(1); // TODO
+
+    /** Represents the wheel steered by the motor. */
+    private final FlywheelSim wheelSim = new FlywheelSim(motorSim, MK4iConstants.STEER_GEARING, MK4iConstants.STEER_MOI);
+
+    /** Represents the angle of the steer motor. */
+    private double angleRotations;
+
+    /** Feedback controller for the angle. */
+    private final PIDController angleController = new PIDController(1.0, 0, 0);
+
+    public SteerMotorIOSim() {
+        angleRotations = 0.0;
+
+        angleController.enableContinuousInput(0, 1);
+    }
+
+    @Override
+    public void update(SteerMotorIOValues values) {
+        double voltage = angleController.calculate(angleRotations);
+
+        wheelSim.setInputVoltage(voltage);
+        wheelSim.update(RobotConstants.TICK_PERIOD);
+
+        double omegaRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0;
+        angleRotations += omegaRotationsPerSecond;
+
+        values.angleRotations = angleRotations;
+        values.omegaRotationsPerSecond = omegaRotationsPerSecond;
+    }
+
+    @Override
+    public void setPosition(double angleRotations) {
+        this.angleRotations = 0.0;
+    }
+
+    @Override
+    public void setSetpoint(double angleRotations) {
+        angleController.setSetpoint(angleRotations);
+    }
+
+
+
+}

src/main/java/frc/robot/swerve/Swerve.java

@@ -1,16 +1,37 @@
 package frc.robot.swerve;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.Subsystem;
+import frc.lib.Telemetry;
 
 /** Subsystem class for the swerve subsystem. */
 public class Swerve extends Subsystem {
 
   /** Instance variable for the swerve subsystem singleton. */
   private static Swerve instance = null;
 
+  /** Swerve modules controlled by the swerve subsystem. */
+  private final SwerveModuleIO[] swerveModules = new SwerveModuleIO[4];
+
+  /** Swerve kinematics.  */
+  private final SwerveDriveKinematics swerveKinematics;
+
   /** Creates a new instance of the swerve subsystem. */
-  private Swerve() {}
+  private Swerve() {
+    swerveModules[0] = SwerveFactory.createModule(SwerveConstants.NORTH_WEST_MODULE_CONFIG);
+    swerveModules[1] = SwerveFactory.createModule(SwerveConstants.NORTH_EAST_MODULE_CONFIG);
+    swerveModules[2] = SwerveFactory.createModule(SwerveConstants.SOUTH_EAST_MODULE_CONFIG);
+    swerveModules[3] = SwerveFactory.createModule(SwerveConstants.SOUTH_WEST_MODULE_CONFIG);
+
+    swerveKinematics = new SwerveDriveKinematics(SwerveConstants.NORTH_WEST_MODULE_CONFIG.position(), SwerveConstants.NORTH_EAST_MODULE_CONFIG.position(), SwerveConstants.SOUTH_EAST_MODULE_CONFIG.position(), SwerveConstants.SOUTH_WEST_MODULE_CONFIG.position());
+  }
 
   /**
    * Gets the instance of the swerve subsystem.
@@ -29,5 +50,85 @@ public static Swerve getInstance() {
   public void periodic() {}
 
   @Override
-  public void addToShuffleboard(ShuffleboardTab tab) {}
+  public void addToShuffleboard(ShuffleboardTab tab) {
+    ShuffleboardLayout northWestModule = Telemetry.addColumn(tab, "North West Module");
+
+    northWestModule.addDouble("Angle (deg)", () -> swerveModules[0].getPosition().angle.getDegrees());
+    northWestModule.addDouble("Distance (m)", () -> swerveModules[0].getPosition().distanceMeters);
+
+    ShuffleboardLayout northEastModule = Telemetry.addColumn(tab, "North East Module");
+
+    northEastModule.addDouble("Angle (deg)", () -> swerveModules[1].getPosition().angle.getDegrees());
+    northEastModule.addDouble("Distance (m)", () -> swerveModules[1].getPosition().distanceMeters);
+
+    ShuffleboardLayout southEastModule = Telemetry.addColumn(tab, "South East Module");
+
+    southEastModule.addDouble("Angle (deg)", () -> swerveModules[2].getPosition().angle.getDegrees());
+    southEastModule.addDouble("Distance (m)", () -> swerveModules[2].getPosition().distanceMeters);
+
+    ShuffleboardLayout southWestModule = Telemetry.addColumn(tab, "South West Module");
+
+    southWestModule.addDouble("Angle (deg)", () -> swerveModules[3].getPosition().angle.getDegrees());
+    southWestModule.addDouble("Distance (m)", () -> swerveModules[3].getPosition().distanceMeters);
+  }
+
+  /**
+   * Sets the swerve's speeds.
+   * 
+   * @param speeds the swerve's speeds.
+   */
+  public void setChassisSpeeds(ChassisSpeeds speeds) {
+    SwerveModuleState[] setpoints = swerveKinematics.toSwerveModuleStates(speeds);
+
+    setSetpoints(setpoints, true);
+  }
+
+  /**
+   * Set the setpoints for each of the swerve's modules. 
+   * 
+   * @param setpoints the setpoints for each of the swerve's modules.
+   * @param lazy if true, optimize the module setpoint.
+   */
+  public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) {
+    SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, SwerveConstants.MAXIMUM_SPEED);
+
+    for (int i = 0; i < 4; i++) {
+      swerveModules[i].setSetpoint(setpoints[i], lazy);
+    }
+  }
+
+  /**
+   * Orients all swerve modules.
+   * 
+   * @param orientations orientations for each swerve modules.
+   * @return a command that orients all swerve modules.
+   */
+  public Command orientModules(Rotation2d[] orientations) {
+    return Commands.run(() -> {
+      setSetpoints(new SwerveModuleState[] {
+        new SwerveModuleState(0.0, orientations[0]),
+        new SwerveModuleState(0.0, orientations[1]),
+        new SwerveModuleState(0.0, orientations[2]),
+        new SwerveModuleState(0.0, orientations[3]),
+      }, false);
+    });
+  }
+
+  public Command forwards() {
+    return orientModules(new Rotation2d[] {
+      Rotation2d.fromDegrees(0.0),
+      Rotation2d.fromDegrees(0.0),
+      Rotation2d.fromDegrees(0.0),
+      Rotation2d.fromDegrees(0.0)
+    });
+  }
+
+  public Command sideways() {
+    return orientModules(new Rotation2d[] {
+      Rotation2d.fromDegrees(90.0),
+      Rotation2d.fromDegrees(90.0),
+      Rotation2d.fromDegrees(90.0),
+      Rotation2d.fromDegrees(90.0)
+    });
+  }
 }