Gil swerve

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

@@ -4,6 +4,21 @@ public class RobotMap {
 
     public interface CAN {
 
+        int FRONT_LEFT_DRIVE_TALON_FX = -1;
+        int FRONT_RIGHT_DRIVE_TALON_FX = -1;
+        int BACK_LEFT_DRIVE_TALON_FX = -1;
+        int BACK_RIGHT_DRIVE_TALON_FX = -1;
+
+        int FRONT_LEFT_TURN_SPARK_MAX = -1;
+        int FRONT_RIGHT_TURN_SPARK_MAX = -1;
+        int BACK_LEFT_TURN_SPARK_MAX = -1;
+        int BACK_RIGHT_TURN_SPARK_MAX = -1;
+
+        int FRONT_LEFT_ABSOLUTE_ENCODER = -1;
+        int FRONT_RIGHT_ABSOLUTE_ENCODER = -1;
+        int BACK_LEFT_ABSOLUTE_ENCODER = -1;
+        int BACK_RIGHT_ABSOLUTE_ENCODER = -1;
+
     }
 
     public interface DIO {

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

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Drivetrain;
+
+import java.util.function.Supplier;
+
+public class Drive extends Command {
+    private final Drivetrain drivetrain;
+    private final Supplier<Double> xSpeed;
+    private final Supplier<Double> ySpeed;
+    private final Supplier<Double> rotationsSpeed;
+    private final boolean fieldRelative;
+    private final boolean usePID;
+
+    public Drive( Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rotationsSpeed, boolean fieldRelative, boolean usePID) {
+        this.drivetrain = drivetrain;
+        this.xSpeed = xSpeed;
+        this.ySpeed = ySpeed;
+        this.rotationsSpeed = rotationsSpeed;
+        this.fieldRelative = fieldRelative;
+        this.usePID = usePID;
+    }
+
+    @Override
+    public void execute() {
+        double xSpeed = this.xSpeed.get();
+        double ySpeed = this.ySpeed.get();
+        double rotationSpeed = this.rotationsSpeed.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,83 @@
+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;
+import frc.robot.subsystems.SwerveModule;
+
+public class Drivetrain extends DashboardedSubsystem {
+    private static final double TRACK_WIDTH = 0.61;
+    private static final double TRACK_LENGTH = 0.58;
+
+    public static final Translation2d CENTER_OF_ROBOT = new Translation2d(0, 0);
+    public static final Translation2d FRONT_LEFT_WHEEL_POSITION =
+            new Translation2d(TRACK_WIDTH / 2, TRACK_LENGTH / 2);
+    public static final Translation2d FRONT_RIGHT_WHEEL_POSITION =
+            new Translation2d(TRACK_WIDTH / 2, -TRACK_LENGTH / 2);
+    public static final Translation2d BACK_LEFT_WHEEL_POSITION =
+            new Translation2d(-TRACK_WIDTH / 2, TRACK_LENGTH / 2);
+    public 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 SwerveDriveKinematics kinematics;
+    private final AHRS gyro;
+
+    public 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.kinematics = new SwerveDriveKinematics(FRONT_LEFT_WHEEL_POSITION,
+                FRONT_RIGHT_WHEEL_POSITION, BACK_LEFT_WHEEL_POSITION, BACK_RIGHT_WHEEL_POSITION);
+        this.gyro = gyro;
+    }
+
+    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, SwerveModule.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 resetRelativeEncoder() {
+        frontLeft.resetRelativeEncoder();
+        frontRight.resetRelativeEncoder();
+        backLeft.resetRelativeEncoder();
+        backRight.resetRelativeEncoder();
+    }
+
+
+    @Override
+    public void configureDashboard() {
+
+    }
+}

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

@@ -0,0 +1,175 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.spikes2212.command.DashboardedSubsystem;
+import com.spikes2212.control.FeedForwardSettings;
+import com.spikes2212.control.PIDSettings;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class SwerveModule extends DashboardedSubsystem {
+
+    public static final double MAX_SPEED = 4;
+    public static final double MIN_SPEED = 0.4;
+    public static final double MAX_TURN = 3;
+    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 = 4 * 0.0254;
+    private static final double DEGREES_IN_ROTATIONS = 360;
+    private static final double SECONDS_IN_MINUTE = 60;
+    private static final double ABSOLUTE_POSITION = 1;
+
+
+    private final TalonFX driveMotor;
+    private final SparkMax turnMotor;
+    private final CANcoder absoluteEncoder;
+    private final boolean cancoderInverted;
+    private final boolean driveInverted;
+    private final double offset;
+    private final RelativeEncoder turnEncoder;
+    private final PIDSettings drivePIDSettings;
+    private final PIDSettings turnPIDSettings;
+    private final FeedForwardSettings driveFeedForwardSettings;
+    private final FeedForwardSettings turnFeedForwardSettings;
+    private final EncoderConfig turnEncoderConfig;
+    private final MotorOutputConfigs MotorOutput;
+
+    public SwerveModule(String namespace, TalonFX driveMotor, SparkMax 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.turnEncoder = turnMotor.getEncoder();
+        this.drivePIDSettings = drivePIDSettings;
+        this.turnPIDSettings = turnPIDSettings;
+        this.driveFeedForwardSettings = driveFeedForwardSettings;
+        this.turnFeedForwardSettings = turnFeedForwardSettings;
+        MotorOutput = new MotorOutputConfigs();
+        turnEncoderConfig = new EncoderConfig();
+        createDriveConfig();
+        createTurnController();
+        configureAbsoluteEncoder();
+    }
+
+    private void createDriveConfig() {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        //@TODO check if this is correct
+        config.Feedback.SensorToMechanismRatio = DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI;
+        config.Feedback.RotorToSensorRatio = (DRIVE_GEAR_RATIO * WHEEL_DIAMETER * Math.PI) / SECONDS_IN_MINUTE;
+    }
+
+
+    public void configureDriveController() {
+        Slot0Configs driveConfigs = new Slot0Configs();
+        driveConfigs.kP = drivePIDSettings.getkP();
+        driveConfigs.kI = drivePIDSettings.getkI();
+        driveConfigs.kD = drivePIDSettings.getkD();
+        driveConfigs.kS = driveFeedForwardSettings.getkS();
+        driveConfigs.kV = driveFeedForwardSettings.getkV();
+        driveConfigs.kA = driveFeedForwardSettings.getkA();
+        driveMotor.getConfigurator().apply(driveConfigs);
+        driveMotor.getConfigurator().apply(MotorOutput.withInverted(driveInverted ?
+                InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive));
+    }
+
+    private void createTurnController() {
+        turnEncoderConfig.positionConversionFactor(DRIVE_GEAR_RATIO * DEGREES_IN_ROTATIONS);
+        turnEncoderConfig.velocityConversionFactor((TURN_GEAR_RATIO * DEGREES_IN_ROTATIONS) / SECONDS_IN_MINUTE);
+    }
+
+    public void configureTurnController() {
+        ClosedLoopConfig turnClosedLoopConfig = new ClosedLoopConfig();
+        turnClosedLoopConfig.pid(turnPIDSettings.getkP(), turnPIDSettings.getkI(), turnPIDSettings.getkD());
+        turnEncoderConfig.inverted(cancoderInverted);
+    }
+
+    public void configureAbsoluteEncoder() {
+        MagnetSensorConfigs magnetConfigs = new MagnetSensorConfigs()
+                .withAbsoluteSensorDiscontinuityPoint(ABSOLUTE_POSITION)
+                .withSensorDirection(cancoderInverted ? SensorDirectionValue.Clockwise_Positive :
+                        SensorDirectionValue.CounterClockwise_Positive).withMagnetOffset(offset);
+        absoluteEncoder.getConfigurator().apply(magnetConfigs);
+    }
+
+    private void setSpeed(double speed, boolean usePID) {
+        if (usePID) {
+            configureDriveController();
+            driveMotor.setControl(new VelocityDutyCycle(speed));
+        } else driveMotor.set(speed / MAX_SPEED);
+    }
+
+    private void setAngle(double angle) {
+        configureTurnController();
+        turnMotor.getClosedLoopController().setReference(angle, SparkBase.ControlType.kVelocity);
+    }
+
+    public void stop() {
+        driveMotor.stopMotor();
+        turnMotor.stopMotor();
+    }
+
+    public void set(SwerveModuleState state, boolean usePID) {
+        if (state.speedMetersPerSecond < MIN_SPEED){
+            stop();
+            return;
+        }
+        state = optimize(state, turnEncoder.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) > 90) {
+            if (desiredAngle - currentAngle > 0) {
+                desiredAngle -= 180;
+            }
+            else {
+                desiredAngle += 180;
+            }
+            state.speedMetersPerSecond *= -1;
+        }
+        return new SwerveModuleState(state.speedMetersPerSecond, Rotation2d.fromDegrees(desiredAngle));
+    }
+
+    public double getAbsoluteAngle() {
+        return absoluteEncoder.getAbsolutePosition().getValueAsDouble() * DEGREES_IN_ROTATIONS;
+    }
+
+    public void resetRelativeEncoder() {
+        turnEncoder.setPosition(getAbsoluteAngle());
+    }
+
+    @Override
+    public void configureDashboard() {
+        namespace.putNumber("absolute angle", this::getAbsoluteAngle);
+        namespace.putNumber("relative angle", turnEncoder::getPosition);
+    }
+}