Basic swerve (#8)

* add PhoenixDriveConstants

* add drive constants file and PhoenixDrive empty class

* add very basic PhoenixDrive example;

* fix format

* git ignore build constants

.gitignore

@@ -176,3 +176,6 @@ logs/
 
 # Folder that has CTRE Phoenix Sim device config storage
 ctre_sim/
+
+
+src/main/java/frc/robot/BuildConstants.java

.vscode/settings.json

@@ -25,5 +25,9 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.compile.nullAnalysis.mode": "automatic",
+  "cSpell.words": [
+    "odometry"
+  ]
 }

src/main/java/frc/robot/constants/PhoenixDriveConstants.java

@@ -0,0 +1,182 @@
+package frc.robot.constants;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.PhoenixDrive;
+
+public final class PhoenixDriveConstants {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with the
+    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+    public static final Slot0Configs steerGains =
+            new Slot0Configs()
+                    .withKP(150)
+                    .withKI(50)
+                    .withKD(0.2)
+                    .withKS(0.25)
+                    .withKV(1.5)
+                    .withKA(0);
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+    public static final Slot0Configs driveGains =
+            new Slot0Configs()
+                    .withKP(0)
+                    .withKI(0.02)
+                    .withKD(0)
+                    .withKS(0.26)
+                    .withKV(0.12)
+                    .withKA(0.01);
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+    // The stator current at which the wheels start to slip;
+    // This needs to be tuned to your individual robot
+    private static final double kSlipCurrentA = 80;
+
+    // Theoretical free speed (m/s) at 12v applied output;
+    // This needs to be tuned to your individual robot
+    public static final double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02
+
+    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+    // This may need to be tuned to your individual robot
+    private static final double kCoupleRatio = 3.5714285714285716;
+
+    private static final double kDriveGearRatio = 6.122448979591837;
+    private static final double kSteerGearRatio = 21.428571428571427;
+    private static final double kWheelRadiusInches = 1.965;
+
+    private static final boolean kSteerMotorReversed = true;
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
+
+    private static final String kCANbusName = "Canivore";
+    private static final int kPigeonId = 1;
+
+    // These are only used for simulation
+    private static final double kSteerInertia = 0.00001;
+    private static final double kDriveInertia = 0.001;
+    // Simulated voltage necessary to overcome friction
+    private static final double kSteerFrictionVoltage = 0.25;
+    private static final double kDriveFrictionVoltage = 0.25;
+
+    private static final SwerveDrivetrainConstants DrivetrainConstants =
+            new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName);
+
+    private static final SwerveModuleConstantsFactory ConstantCreator =
+            new SwerveModuleConstantsFactory()
+                    .withDriveMotorGearRatio(kDriveGearRatio)
+                    .withSteerMotorGearRatio(kSteerGearRatio)
+                    .withWheelRadius(kWheelRadiusInches)
+                    .withSlipCurrent(kSlipCurrentA)
+                    .withSteerMotorGains(steerGains)
+                    .withDriveMotorGains(driveGains)
+                    .withSteerMotorClosedLoopOutput(steerClosedLoopOutput)
+                    .withDriveMotorClosedLoopOutput(driveClosedLoopOutput)
+                    .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps)
+                    .withSteerInertia(kSteerInertia)
+                    .withDriveInertia(kDriveInertia)
+                    .withSteerFrictionVoltage(kSteerFrictionVoltage)
+                    .withDriveFrictionVoltage(kDriveFrictionVoltage)
+                    .withFeedbackSource(SteerFeedbackType.FusedCANcoder)
+                    .withCouplingGearRatio(kCoupleRatio)
+                    .withSteerMotorInverted(kSteerMotorReversed);
+
+    // Front Left
+    private static final int kBackRightDriveMotorId = 2;
+    private static final int kBackRightSteerMotorId = 1;
+    private static final int kBackRightEncoderId = 1;
+    private static final double kBackRightEncoderOffset = 0.3486328125;
+
+    private static final double kBackRightXPosInches = 10.375;
+    private static final double kBackRightYPosInches = 10.375;
+
+    // Front Right
+    private static final int kBackLeftDriveMotorId = 4;
+    private static final int kBackLeftSteerMotorId = 3;
+    private static final int kBackLeftEncoderId = 2;
+    private static final double kBackLeftEncoderOffset = 0.096435546875;
+
+    private static final double kBackLeftXPosInches = 10.375;
+    private static final double kBackLeftYPosInches = -10.375;
+
+    // Back Left
+    private static final int kFrontRightDriveMotorId = 8;
+    private static final int kFrontRightSteerMotorId = 7;
+    private static final int kFrontRightEncoderId = 4;
+    private static final double kFrontRightEncoderOffset = 0.130859375;
+
+    private static final double kFrontRightXPosInches = -10.375;
+    private static final double kFrontRightYPosInches = 10.375;
+
+    // Back Right
+    private static final int kFrontLeftDriveMotorId = 6;
+    private static final int kFrontLeftSteerMotorId = 5;
+    private static final int kFrontLeftEncoderId = 3;
+    private static final double kFrontLeftEncoderOffset = -0.372802734375;
+
+    private static final double kFrontLeftXPosInches = -10.375;
+    private static final double kFrontLeftYPosInches = -10.375;
+
+    public static final double kModuleRadiusMeters =
+            Units.inchesToMeters(Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches));
+
+    private static final SwerveModuleConstants FrontLeft =
+            ConstantCreator.createModuleConstants(
+                    kFrontLeftSteerMotorId,
+                    kFrontLeftDriveMotorId,
+                    kFrontLeftEncoderId,
+                    kFrontLeftEncoderOffset,
+                    Units.inchesToMeters(kFrontLeftXPosInches),
+                    Units.inchesToMeters(kFrontLeftYPosInches),
+                    kInvertLeftSide);
+    private static final SwerveModuleConstants FrontRight =
+            ConstantCreator.createModuleConstants(
+                    kFrontRightSteerMotorId,
+                    kFrontRightDriveMotorId,
+                    kFrontRightEncoderId,
+                    kFrontRightEncoderOffset,
+                    Units.inchesToMeters(kFrontRightXPosInches),
+                    Units.inchesToMeters(kFrontRightYPosInches),
+                    kInvertRightSide);
+    private static final SwerveModuleConstants BackLeft =
+            ConstantCreator.createModuleConstants(
+                    kBackLeftSteerMotorId,
+                    kBackLeftDriveMotorId,
+                    kBackLeftEncoderId,
+                    kBackLeftEncoderOffset,
+                    Units.inchesToMeters(kBackLeftXPosInches),
+                    Units.inchesToMeters(kBackLeftYPosInches),
+                    kInvertLeftSide);
+    private static final SwerveModuleConstants BackRight =
+            ConstantCreator.createModuleConstants(
+                    kBackRightSteerMotorId,
+                    kBackRightDriveMotorId,
+                    kBackRightEncoderId,
+                    kBackRightEncoderOffset,
+                    Units.inchesToMeters(kBackRightXPosInches),
+                    Units.inchesToMeters(kBackRightYPosInches),
+                    kInvertRightSide);
+
+    public static final SwerveDriveKinematics kinematics =
+            new SwerveDriveKinematics(
+                    new Translation2d(FrontLeft.LocationX, FrontLeft.LocationY),
+                    new Translation2d(FrontLeft.LocationX, FrontRight.LocationY),
+                    new Translation2d(BackLeft.LocationX, BackLeft.LocationY),
+                    new Translation2d(BackRight.LocationX, BackRight.LocationY));
+
+    public static final PhoenixDrive DriveTrain =
+            new PhoenixDrive(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
+}

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

@@ -0,0 +1,77 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.function.Supplier;
+
+public class PhoenixDrive extends SwerveDrivetrain implements Subsystem {
+    private static final double kSimLoopPeriod = 0.005; // 5 ms
+    private Notifier m_simNotifier = null;
+    private double m_lastSimTime;
+
+    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
+    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180);
+    /* Keep track if we've ever applied the operator perspective before or not */
+    private boolean hasAppliedOperatorPerspective = false;
+
+    private final SwerveRequest.ApplyChassisSpeeds AutoRequest =
+            new SwerveRequest.ApplyChassisSpeeds();
+
+    private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization =
+            new SwerveRequest.SysIdSwerveTranslation();
+    private final SwerveRequest.SysIdSwerveRotation RotationCharacterization =
+            new SwerveRequest.SysIdSwerveRotation();
+    private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization =
+            new SwerveRequest.SysIdSwerveSteerGains();
+
+    public PhoenixDrive(
+            SwerveDrivetrainConstants driveConstants,
+            double odometryUpdateFrequency,
+            SwerveModuleConstants... modules) {
+        super(driveConstants, odometryUpdateFrequency, modules);
+
+        CommandScheduler.getInstance().registerSubsystem(this);
+    }
+
+    public PhoenixDrive(
+            SwerveDrivetrainConstants driveConstants, SwerveModuleConstants... modules) {
+        super(driveConstants, modules);
+
+        CommandScheduler.getInstance().registerSubsystem(this);
+    }
+
+    public Command applyDriveRequest(Supplier<SwerveRequest> requestSupplier) {
+        return run(() -> this.setControl(requestSupplier.get()));
+    }
+
+    public ChassisSpeeds getCurrentSpeeds() {
+        return m_kinematics.toChassisSpeeds(getState().ModuleStates);
+    }
+
+    @Override
+    public void periodic() {
+        if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+            DriverStation.getAlliance()
+                    .ifPresent(
+                            (color) -> {
+                                this.setOperatorPerspectiveForward(
+                                        color == Alliance.Red
+                                                ? RedAlliancePerspectiveRotation
+                                                : BlueAlliancePerspectiveRotation);
+                            });
+            hasAppliedOperatorPerspective = true;
+        }
+    }
+}