Gil swerve

build.gradle

@@ -85,7 +85,6 @@ dependencies {
     implementation group: 'com.ctre.phoenix6', name: 'wpiapi-java', version: '24.0.0-beta-8'
     implementation group: 'com.ctre.phoenix', name: 'api-java', version: '5.32.0-beta-5'
     implementation group: 'com.ctre.phoenix', name: 'wpiapi-java', version: '5.31.0'
-    implementation 'com.github.Spikes-2212-Programming-Guild:SpikesLib2:update-wpilib-25-SNAPSHOT'
 }
 
 test {

src/main/java/frc/robot/RobotMap.java

@@ -4,6 +4,20 @@ public class RobotMap {
 
     public interface CAN {
 
+        int FRONT_LEFT_DRIVE_TALON_FX = 1;
+        int FRONT_RIGHT_DRIVE_TALON_FX = 4;
+        int BACK_LEFT_DRIVE_TALON_FX = 7;
+        int BACK_RIGHT_DRIVE_TALON_FX = 10;
+
+        int FRONT_LEFT_TURN_SPARK_MAX = 2;
+        int FRONT_RIGHT_TURN_SPARK_MAX = 5;
+        int BACK_LEFT_TURN_SPARK_MAX = 8;
+        int BACK_RIGHT_TURN_SPARK_MAX = 11;
+
+        int FRONT_LEFT_ABSOLUTE_ENCODER = 3;
+        int FRONT_RIGHT_ABSOLUTE_ENCODER = 6;
+        int BACK_LEFT_ABSOLUTE_ENCODER = 9;
+        int BACK_RIGHT_ABSOLUTE_ENCODER = 12;
     }
 
     public interface DIO {

src/main/java/frc/robot/commands/Drive.java

@@ -0,0 +1,60 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Drivetrain;
+
+import java.util.function.Supplier;
+
+public class Drive extends Command {
+
+    private final double DRIVE_ACCELERATION_LIMIT = 2;
+    private final double TURN_ACCELERATION_LIMIT = 4;
+
+    private final Drivetrain drivetrain;
+    private final Supplier<Double> xSpeed;
+    private final Supplier<Double> ySpeed;
+    private final Supplier<Double> rotationSpeed;
+    private final boolean fieldRelative;
+    private final boolean usePID;
+
+    private final SlewRateLimiter xLimiter;
+    private final SlewRateLimiter yLimiter;
+    private final SlewRateLimiter rotationLimiter;
+    private final boolean limitAcceleration;
+
+    public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
+                 Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,
+                 boolean limitAcceleration) {
+        addRequirements(drivetrain);
+        this.drivetrain = drivetrain;
+        this.xSpeed = xSpeed;
+        this.ySpeed = ySpeed;
+        this.rotationSpeed = rotationSpeed;
+        this.usePID = usePID;
+        this.limitAcceleration = limitAcceleration;
+        this.fieldRelative = fieldRelative;
+        xLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        yLimiter = new SlewRateLimiter(DRIVE_ACCELERATION_LIMIT);
+        rotationLimiter = new SlewRateLimiter(TURN_ACCELERATION_LIMIT);
+    }
+
+    @Override
+    public void execute() {
+        double xSpeed = this.xSpeed.get();
+        double ySpeed = this.ySpeed.get();
+        double rotationSpeed = this.rotationSpeed.get();
+
+        if (limitAcceleration) {
+            xSpeed = xLimiter.calculate(this.xSpeed.get());
+            ySpeed = yLimiter.calculate(this.ySpeed.get());
+            rotationSpeed = rotationLimiter.calculate(this.rotationSpeed.get());
+        }
+
+        drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID);
+    }
+
+    public void end(boolean interrupted) {
+        drivetrain.stop();
+    }
+}

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

@@ -0,0 +1,102 @@
+package frc.robot.subsystems;
+
+import com.spikes2212.command.DashboardedSubsystem;
+import com.studica.frc.AHRS;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class Drivetrain extends DashboardedSubsystem {
+
+    public static final double MAX_SPEED = 4;
+    public static final double MIN_SPEED = 0.05;
+    public static final double MAX_TURN_SPEED = 3;
+
+    private static final double TRACK_WIDTH = 0.6;
+    private static final double TRACK_LENGTH = 0.6;
+
+    private static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
+    private static final Translation2d FRONT_LEFT_WHEEL_POSITION =
+            new Translation2d(TRACK_WIDTH / 2, TRACK_LENGTH / 2);
+    private static final Translation2d FRONT_RIGHT_WHEEL_POSITION =
+            new Translation2d(TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
+    private static final Translation2d BACK_LEFT_WHEEL_POSITION =
+            new Translation2d(-TRACK_WIDTH / 2, TRACK_LENGTH / 2);
+    private static final Translation2d BACK_RIGHT_WHEEL_POSITION =
+            new Translation2d(-TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
+
+    private static final String NAMESPACE_NAME = "drivetrain";
+
+    private final SwerveModule frontLeft;
+    private final SwerveModule frontRight;
+    private final SwerveModule backLeft;
+    private final SwerveModule backRight;
+    private final AHRS gyro;
+    private final SwerveDriveKinematics kinematics;
+
+    private static Drivetrain instance;
+
+    public static Drivetrain getInstance() {
+        if (instance == null) {
+            instance = new Drivetrain(SwerveModuleHolder.getFrontLeft(), SwerveModuleHolder.getFrontRight(),
+                    SwerveModuleHolder.getBackLeft(), SwerveModuleHolder.getBackRight(),
+                    new AHRS(AHRS.NavXComType.kMXP_SPI));
+        }
+        return instance;
+    }
+
+    private Drivetrain(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft,
+                      SwerveModule backRight, AHRS gyro) {
+        super(NAMESPACE_NAME);
+        this.frontLeft = frontLeft;
+        this.frontRight = frontRight;
+        this.backLeft = backLeft;
+        this.backRight = backRight;
+        this.gyro = gyro;
+        kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
+                FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        configureDashboard();
+    }
+
+    public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative,
+                      boolean usePID) {
+        ChassisSpeeds speeds;
+        if (fieldRelative) {
+            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotationSpeed,
+                    gyro.getRotation2d());
+        }
+        else {
+            speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
+        }
+        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds, CENTER_OF_ROBOT);
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_SPEED);
+        frontLeft.set(states[0], usePID);
+        frontRight.set(states[1], usePID);
+        backLeft.set(states[2], usePID);
+        backRight.set(states[3], usePID);
+    }
+
+    public void stop() {
+        frontLeft.stop();
+        frontRight.stop();
+        backLeft.stop();
+        backRight.stop();
+    }
+
+    public void resetRelativeEncoders() {
+        frontLeft.resetRelativeEncoder();
+        frontRight.resetRelativeEncoder();
+        backLeft.resetRelativeEncoder();
+        backRight.resetRelativeEncoder();
+    }
+
+    public void resetGyro() {
+        gyro.reset();
+    }
+
+    @Override
+    public void configureDashboard() {
+        namespace.putNumber("gyro yaw", gyro::getAngle);
+    }
+}

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

@@ -0,0 +1,144 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.control.FeedForwardSettings;
+import com.spikes2212.control.PIDSettings;
+import com.spikes2212.util.UnifiedControlMode;
+import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
+import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class SwerveModule extends DashboardedSubsystem {
+
+    private static final double DRIVE_GEAR_RATIO = 1 / 6.12;
+    private static final double TURN_GEAR_RATIO = 1 / 12.8;
+    private static final double WHEEL_DIAMETER_INCHES = 4;
+    private static final double INCHES_TO_METERS = 0.0254;
+    private static final double DEGREES_IN_ROTATIONS = 360;
+    private static final double SECONDS_IN_MINUTE = 60;
+
+    private static final double MAX_DISTANCE_TO_ROTATE = 90;
+    private static final double DEGREES_TO_FLIP = 180;
+    private static final double ABSOLUTE_POSITION_DISCONTINUITY_POINT = 1;
+
+    private final TalonFXWrapper driveMotor;
+    private final SparkWrapper turnMotor;
+    private final CANcoder absoluteEncoder;
+
+    private final boolean cancoderInverted;
+    private final boolean driveInverted;
+    private final double offset;
+
+    private final PIDSettings drivePIDSettings;
+    private final PIDSettings turnPIDSettings;
+    private final FeedForwardSettings driveFeedForwardSettings;
+    private final FeedForwardSettings turnFeedForwardSettings;
+
+    public SwerveModule(String namespace, TalonFXWrapper driveMotor, SparkWrapper turnMotor, CANcoder absoluteEncoder,
+                        boolean cancoderInverted, boolean driveInverted, double offset,
+                        PIDSettings drivePIDSettings, PIDSettings turnPIDSettings,
+                        FeedForwardSettings driveFeedForwardSettings,
+                        FeedForwardSettings turnFeedForwardSettings) {
+        super(namespace);
+        this.driveMotor = driveMotor;
+        this.turnMotor = turnMotor;
+        this.absoluteEncoder = absoluteEncoder;
+        this.cancoderInverted = cancoderInverted;
+        this.driveInverted = driveInverted;
+        this.offset = offset;
+        this.drivePIDSettings = drivePIDSettings;
+        this.turnPIDSettings = turnPIDSettings;
+        this.driveFeedForwardSettings = driveFeedForwardSettings;
+        this.turnFeedForwardSettings = turnFeedForwardSettings;
+        configureDriveController();
+        configureTurnController();
+        configureAbsoluteEncoder();
+        configureDashboard();
+    }
+
+    public void configureDriveController() {
+        //@TODO check if this is correct
+        driveMotor.setEncoderConversionFactor(DRIVE_GEAR_RATIO * WHEEL_DIAMETER_INCHES
+                * INCHES_TO_METERS * Math.PI);
+        driveMotor.setInverted(driveInverted);
+    }
+
+    public void configureTurnController() {
+        turnMotor.setPositionConversionFactor(TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnMotor.setVelocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
+        turnMotor.setInverted(cancoderInverted);
+    }
+
+    public void configureAbsoluteEncoder() {
+        MagnetSensorConfigs magnetConfigs = new MagnetSensorConfigs()
+                .withAbsoluteSensorDiscontinuityPoint(ABSOLUTE_POSITION_DISCONTINUITY_POINT)
+                .withSensorDirection(cancoderInverted ? SensorDirectionValue.Clockwise_Positive :
+                        SensorDirectionValue.CounterClockwise_Positive).withMagnetOffset(offset);
+        absoluteEncoder.getConfigurator().apply(magnetConfigs);
+    }
+
+    private void setSpeed(double speed, boolean usePID) {
+        if (usePID) {
+            driveMotor.pidSet(UnifiedControlMode.VELOCITY, speed, drivePIDSettings, driveFeedForwardSettings,
+                    true);
+        } else driveMotor.set(speed / Drivetrain.MAX_SPEED);
+    }
+
+    private void setAngle(double angle) {
+        turnMotor.pidSet(UnifiedControlMode.POSITION, angle, turnPIDSettings, turnFeedForwardSettings,
+                true);
+    }
+
+    public void stop() {
+        driveMotor.stopMotor();
+        turnMotor.stopMotor();
+    }
+
+    public void set(SwerveModuleState state, boolean usePID) {
+        if (Math.abs(state.speedMetersPerSecond) < Drivetrain.MIN_SPEED) {
+            stop();
+            return;
+        }
+        state = optimize(state, turnMotor.getPosition());
+        setAngle(state.angle.getDegrees());
+        setSpeed(state.speedMetersPerSecond, usePID);
+    }
+
+    private double normalizeAngleRelativeToEncoder(double currentAngle, double desiredAngle) {
+        int rotations = (int) (currentAngle / DEGREES_IN_ROTATIONS);
+        if (currentAngle < 0) rotations--;
+        desiredAngle += rotations * DEGREES_IN_ROTATIONS;
+        return desiredAngle;
+    }
+
+    private SwerveModuleState optimize(SwerveModuleState state, double currentAngle) {
+        double desiredAngle = normalizeAngleRelativeToEncoder(currentAngle, state.angle.getDegrees());
+        while (Math.abs(desiredAngle - currentAngle) > MAX_DISTANCE_TO_ROTATE) {
+            if (desiredAngle - currentAngle > 0) {
+                desiredAngle -= DEGREES_TO_FLIP;
+            } else {
+                desiredAngle += DEGREES_TO_FLIP;
+            }
+            state.speedMetersPerSecond *= -1;
+        }
+        return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));
+    }
+
+    public double getAbsoluteAngle() {
+        return absoluteEncoder.getAbsolutePosition().getValueAsDouble() * DEGREES_IN_ROTATIONS;
+    }
+
+    public void resetRelativeEncoder() {
+        turnMotor.setPosition(getAbsoluteAngle());
+    }
+
+    @Override
+    public void configureDashboard() {
+        namespace.putNumber("absolute angle", this::getAbsoluteAngle);
+        namespace.putNumber("relative angle", turnMotor::getPosition);
+    }
+}