dev

src/main/java/frc/robot/sim/swerve/SimSwerveModule.java

@@ -0,0 +1,87 @@
+package frc.robot.sim.swerve;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.sim.hardware.SimMotor; // Utilizing your abstraction
+
+public class SimSwerveModule {
+    private final SimMotor driveMotor;
+    private final SimMotor turnMotor;
+    private final SwerveModuleConfig config;
+
+    // Controllers simulating internal or software-based PID loops
+    private final PIDController drivePID;
+    private final PIDController turnPID;
+
+    public SimSwerveModule(SimMotor driveMotor, SimMotor turnMotor, SwerveModuleConfig config) {
+        this.driveMotor = driveMotor;
+        this.turnMotor = turnMotor;
+        this.config = config;
+
+        // Simple PID controllers for simulation tracking
+        this.drivePID = new PIDController(1.0, 0.0, 0.0);
+        this.turnPID = new PIDController(5.0, 0.0, 0.0);
+
+        // Turn loops are continuous since the module can spin infinitely
+        this.turnPID.enableContinuousInput(-Math.PI, Math.PI);
+    }
+
+    /**
+     * Commands the module to a specific state (velocity and angle).
+     * Calculates the necessary voltages and applies them to the simulated motors.
+     */
+    public void setDesiredState(SwerveModuleState desiredState) {
+        // Optimize the state to avoid turning more than 90 degrees
+        SwerveModuleState optimizedState = SwerveModuleState.optimize(
+                desiredState,
+                getAngle()
+        );
+
+        // Calculate drive motor voltage output based on target velocity (m/s)
+        double currentDriveVelocity = getDriveVelocityMetersPerSec();
+        double driveOutputVolts = drivePID.calculate(currentDriveVelocity, optimizedState.speedMetersPerSecond);
+        driveMotor.setVoltage(driveOutputVolts);
+
+        // Calculate turn motor voltage output based on target angle (radians)
+        double currentTurnAngle = getAngle().getRadians();
+        double turnOutputVolts = turnPID.calculate(currentTurnAngle, optimizedState.angle.getRadians());
+        turnMotor.setVoltage(turnOutputVolts);
+    }
+
+    /** Calculates current module angle based on the turn motor position and gear ratio */
+    public Rotation2d getAngle() {
+        double motorRotations = turnMotor.getPosition();
+        // Handle conversion if your SimMotor outputs raw radians or rotations
+        double moduleRadians = motorRotations / (config.turnGearRatio == -1 ? 1.0 : config.turnGearRatio);
+        return new Rotation2d(moduleRadians);
+    }
+
+    /** Calculates drive velocity in meters per second */
+    public double getDriveVelocityMetersPerSec() {
+        // Wheel diameter or radius would typically be added to the config class
+        double wheelCircumference = 0.1016 * Math.PI; // Standard 4-inch wheel example
+        double motorRotationsPerSec = driveMotor.getVelocity() / (2 * Math.PI);
+        double wheelRotationsPerSec = motorRotationsPerSec / (config.driveGearRatio == -1 ? 1.0 : config.driveGearRatio);
+        return wheelRotationsPerSec * wheelCircumference;
+    }
+
+    /** Calculates total drive distance traveled in meters */
+    public double getDriveDistanceMeters() {
+        double wheelCircumference = 0.1016 * Math.PI;
+        double motorRotations = driveMotor.getPosition() / (2 * Math.PI);
+        double wheelRotations = motorRotations / (config.driveGearRatio == -1 ? 1.0 : config.driveGearRatio);
+        return wheelRotations * wheelCircumference;
+    }
+
+    /** Returns the current state of the module for kinematics calculations */
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(getDriveVelocityMetersPerSec(), getAngle());
+    }
+
+    /** Returns the current position of the module for odometry tracking */
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(getDriveDistanceMeters(), getAngle());
+    }
+}