Added code for swerve wheel support

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Launch Main",
+      "request": "launch",
+      "mainClass": "org.ghrobotics.frc2023.Main",
+      "projectName": "2023CompetitionSeason"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

src/main/java/org/ghrobotics/frc2023/subsystems/2023OffSeasonCompetitionSeason.code-workspace

@@ -0,0 +1,13 @@
+{
+	"folders": [
+		{
+			"path": "../../../../../../.."
+		},
+		{
+			"path": "../../../../../../.."
+		},
+		{
+			"path": "../../../../../../.."
+		}
+	]
+}

src/main/java/org/ghrobotics/frc2023/subsystems/Drive.java

@@ -0,0 +1,128 @@
+package org.ghrobotics.frc2023.subsystems;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SPI;
+import com.kauailabs.navx.frc.AHRS; 
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Drive extends SubsystemBase {
+    private AHRS gyro_;
+    private Pose2d pose = new Pose2d (0.0, 0.0, new Rotation2d());
+    private SwerveModule[] swerveModules = new SwerveModule [] {
+    new SwerveModule(Constants.kFLDriveMotor, Constants.kFLSteerMotor, Constants.kFLCanCoder),
+    new SwerveModule(Constants.kBLDriveMotor, Constants.kBLSteerMotor, Constants.kBLCanCoder),
+    new SwerveModule(Constants.kFRDriveMotor, Constants.kFRSteerMotor, Constants.kBRCanCoder),
+    new SwerveModule(Constants.kBRDriveMotor, Constants.kBRSteerMotor, Constants.kBRCanCoder)
+    };
+
+    SwerveModule frontLeft = swerveModules [0];
+    SwerveModule backLeft = swerveModules [1];
+    SwerveModule frontRight = swerveModules [2];
+    SwerveModule backRight = swerveModules [3];
+
+    Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+    Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+    Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+    Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+    // Creating my kinematics object using the module locations
+    SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+    );
+
+    public SwerveDriveOdometry odometry = new SwerveDriveOdometry (m_kinematics, gyro_.getRotation2d(), new SwerveModulePosition[] {
+        frontLeft.getPosition(), frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition() }, pose);
+
+    @Override
+    public void periodic() {
+        // Get the rotation of the robot from the gyro.
+        var gyroAngle = gyro_.getRotation2d();
+
+        // Update the pose
+        pose = odometry.update((Rotation2d) gyroAngle,
+            new SwerveModulePosition[] {
+            frontLeft.getPosition(), frontRight.getPosition(),
+            backLeft.getPosition(), backRight.getPosition()
+            });
+    }
+
+    public Drive(){
+
+       gyro_ = new AHRS(SPI.Port.kMXP);
+
+        new Thread(() -> {
+            try {
+                Thread.sleep(1000);
+                zeroHeading();
+        } catch (Exception e) {
+        }
+        }).start();
+    }
+
+    public void zeroHeading(){
+        gyro_.reset();
+    }
+
+    public double getHeading() {
+        return Math.IEEEremainder(gyro_.getAngle(), 360);
+    }
+
+    public Rotation2d getRotation2d() {
+        return Rotation2d.fromDegrees(getHeading());
+    }
+
+    public Pose2d getPose() {
+        return odometry.getPoseMeters();
+    }
+
+    public SwerveModulePosition[] getModulePositions(){
+        SwerveModulePosition [] positions = new SwerveModulePosition[4];
+        for (SwerveModule mod: swerveModules){
+            positions[mod.moduleNumber] = mod.getPosition();
+        }
+        return positions;
+    }
+
+    public void stopModules() {
+        frontLeft.stop();
+        frontRight.stop();
+        backLeft.stop();
+        backRight.stop();
+    }
+
+    public void setModuleStates(SwerveModuleState[] desiredStates) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.maxSpeed);
+        frontLeft.setDesiredState(desiredStates[0]);
+        frontRight.setDesiredState(desiredStates[1]);
+        backLeft.setDesiredState(desiredStates[2]);
+        backRight.setDesiredState(desiredStates[3]);
+    }
+    public static class Constants{
+        //Drive Motors
+        public static final int kFLDriveMotor = 0;
+        public static final int kFRDriveMotor = 0;
+        public static final int kBLDriveMotor = 0;
+        public static final int kBRDriveMotor = 0;
+
+        //Steer Motors
+        public static final int kFLSteerMotor = 0;
+        public static final int kFRSteerMotor = 0;
+        public static final int kBLSteerMotor = 0;
+        public static final int kBRSteerMotor = 0;
+
+        //CanCodrs
+        public static final int kFLCanCoder = 0;
+        public static final int kFRCanCoder = 0;
+        public static final int kBLCanCoder = 0;
+        public static final int kBRCanCoder = 0;
+
+        public static final double maxSpeed = 0.5;
+    }
+
+}

src/main/java/org/ghrobotics/frc2023/subsystems/SwerveModule.java

@@ -0,0 +1,155 @@
+package org.ghrobotics.frc2023.subsystems;
+
+
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+import com.ctre.phoenix.sensors.SensorTimeBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
+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 edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SwerveModule extends SubsystemBase{
+    public int moduleNumber;
+
+    //Motor Controllers
+    private final CANSparkMax drive_;
+    private final CANSparkMax steer_;
+
+    //Sensors
+    private final RelativeEncoder drive_encoder_;
+    private final RelativeEncoder steer_encoder_;
+    private final CANCoder cancoder_;
+
+    // Control
+    private final PIDController drive_pid_controller_;
+    private final PIDController position_pid_controller_;
+
+    public SwerveModule(int driveMotorID, int steerMotorID, int CanCoderID) {
+        // Initialize motor controllers
+        drive_ = new CANSparkMax(driveMotorID, MotorType.kBrushless);
+        drive_.restoreFactoryDefaults();
+        drive_.setIdleMode(IdleMode.kBrake);
+        drive_.enableVoltageCompensation(12);
+        drive_.setInverted(true);
+
+        steer_ = new CANSparkMax(steerMotorID, MotorType.kBrushless);
+        steer_.restoreFactoryDefaults();
+        steer_.setIdleMode(IdleMode.kBrake);
+        steer_.enableVoltageCompensation(12);
+        steer_.setInverted(false);
+
+        // Initialize encoders
+        drive_encoder_ = drive_.getEncoder();
+        drive_encoder_.setPositionConversionFactor(
+            2 * Math.PI * Constants.kWheelRadius / Constants.kGearRatio);
+        drive_encoder_.setVelocityConversionFactor(
+            2 * Math.PI * Constants.kWheelRadius / Constants.kGearRatio / 60);
+
+        steer_encoder_ = steer_.getEncoder();
+        steer_encoder_.setPositionConversionFactor(
+            2 * Math.PI * Constants.kWheelRadius / Constants.kGearRatio);
+        steer_encoder_.setVelocityConversionFactor(
+            2 * Math.PI * Constants.kWheelRadius / Constants.kGearRatio / 60);
+
+        cancoder_ = new CANCoder(CanCoderID);
+        CANCoderConfiguration config = new CANCoderConfiguration();
+        // set units of the CANCoder to radians, with velocity being radians per second
+        config.sensorCoefficient = 2 * Math.PI / 4096.0;
+        config.unitString = "rad";
+        config.sensorTimeBase = SensorTimeBase.PerSecond;
+        cancoder_.configAllSettings(config);
+
+        // Initialize PID controllers.
+        drive_pid_controller_ = new PIDController (Constants.kDriveKp, Constants.kI, Constants.kD);
+        position_pid_controller_ = new PIDController (Constants.kSteerKp, Constants.kI, Constants.kD);
+        position_pid_controller_.enableContinuousInput (-Math.PI, Math.PI);
+    }
+
+    //getMethods
+    public double DrivePosition(){
+        return drive_encoder_.getPosition();
+    }
+
+    public double getDriveVelocity(){
+        return drive_encoder_.getVelocity();
+    }
+    public double getSteerPosition(){
+        return steer_encoder_.getPosition();
+    }
+
+    public double getSteerVelocity(){
+        return steer_encoder_.getVelocity();
+    }
+
+    public double getCanCoderPosition(){
+        return cancoder_.getAbsolutePosition();
+    }
+
+    //Setting the state of the module
+    public void setDesiredState (SwerveModuleState desiredState){
+        desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
+        setAngle(desiredState);
+        setSpeed(desiredState);
+    }
+
+    //Sets angle
+    public void setAngle (SwerveModuleState desiredState){
+        steer_.set(position_pid_controller_.calculate(getCanCoderPosition(), desiredState.angle.getRadians()));
+    }
+
+    //Sets speed
+    public void setSpeed(SwerveModuleState desiredState){
+        if (Math.abs(desiredState.speedMetersPerSecond) <0.001) {
+            stop();
+            return;
+        }
+        desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
+        double desiredDrive = desiredState.speedMetersPerSecond / Constants.kMaxSpeed;
+        drive_.set(drive_pid_controller_.calculate(getDriveVelocity(), desiredDrive));
+    }
+
+    public void stop(){
+        drive_.set(0);
+        steer_.set(0); 
+    }
+    public SwerveModuleState getState(){
+        return new SwerveModuleState(getDriveVelocity(), new Rotation2d(getCanCoderPosition()));
+    }
+
+    //Does this get module position (distance and angle) from each module?
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+            getDriveVelocity(), new Rotation2d(getSteerPosition()));
+      }
+
+    public static class Constants {
+
+        // Hardware
+        public static double kGearRatio = 0.0;
+        public static double kWheelRadius = 0.0;
+        public static double kTrackWidth = 0.0;
+
+        public static final double kKs = 0.0;
+        public static final double kKv = 0.0;
+        public static final double kKa = 0.0;
+
+        public static final double kI = 0.0;
+        public static final double kD = 0.0;
+
+        public static final double kDriveKp = 0.0;
+        public static final double kSteerKp = 0.0;
+
+        // Output Limit
+        public static final double kOutputLimit = 0.0;
+
+        public static final double kMaxSpeed = 0.8;
+  }
+
+}
+

vendordeps/NavX.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "NavX.json",
+    "name": "KauaiLabs_navX_FRC",
+    "version": "2023.0.3",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2023/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json",
+    "javaDependencies": [{
+        "groupId": "com.kauailabs.navx.frc",
+        "artifactId": "navx-frc-java",
+        "version": "2023.0.3"
+    }],
+    "jniDependencies": [],
+    "cppDependencies": [{
+        "groupId": "com.kauailabs.navx.frc",
+        "artifactId": "navx-frc-cpp",
+        "version": "2023.0.3",
+        "headerClassifier": "headers",
+        "sourcesClassifier": "sources",
+        "sharedLibrary": false,
+        "libName": "navx_frc",
+        "skipInvalidPlatforms": true,
+        "binaryPlatforms": [
+            "linuxathena",
+            "linuxraspbian",
+            "linuxarm32",
+            "linuxarm64",
+            "linux86-64",
+            "osxuniversal",
+            "windowsx86-64"
+        ]
+    }]
+}