Add initial implementation of Swerve Drive system and configuration f…

src/main/deploy/swerve/modules/controllerproperties.json

@@ -0,0 +1,8 @@
+{
+    "angleJoystickRadiusDeadband": 0.5,
+    "heading": {
+      "p": 0.4,
+      "i": 0,
+      "d": 0.01
+    }
+  }

src/main/deploy/swerve/modules/physicalproperties.json

@@ -0,0 +1,16 @@
+{
+    "conversionFactors": {
+      "angle": {"gearRatio": 28.125},
+      "drive": {"diameter": 4, "gearRatio": 6.75}
+    },
+    "currentLimit": {
+      "drive": 40,
+      "angle": 20
+    },
+    "rampRate": {
+      "drive": 0.25,
+      "angle": 0.25
+    },
+    "wheelGripCoefficientOfFriction": 1.19,
+    "optimalVoltage": 12
+  }

src/main/deploy/swerve/modules/pidfproperties.json

@@ -0,0 +1,16 @@
+{
+    "drive": {
+      "p": 0.0020645,
+      "i": 0,
+      "d": 0,
+      "f": 0,
+      "iz": 0
+    },
+    "angle": {
+      "p": 0.01,
+      "i": 0,
+      "d": 0,
+      "f": 0,
+      "iz": 0
+    }
+  }

src/main/deploy/swerve/modules/swervedrive.json

@@ -0,0 +1,14 @@
+{
+    "imu": {
+      "type": "pigeon2",
+      "id": 13,
+      "canbus": "canivore"
+    },
+    "invertedIMU": true,
+    "modules": [
+      "frontleft.json",
+      "frontright.json",
+      "backleft.json",
+      "backright.json"
+    ]
+  }

src/main/deploy/swerve/modules/x.json

@@ -0,0 +1,27 @@
+{
+  "drive": {
+    "type": "sparkmax",
+    "id": 2,
+    "canbus": null
+  },
+  "angle": {
+    "type": "sparkmax",
+    "id": 1,
+    "canbus": null
+  },
+  "encoder": {
+    "type": "cancoder",
+    "id": 10,
+    "canbus": null
+  },
+  "inverted": {
+    "drive": false,
+    "angle": false
+  },
+  "absoluteEncoderInverted": false,
+  "absoluteEncoderOffset": -50.977,
+  "location": {
+    "front": 12,
+    "left": -12
+  }
+}

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

@@ -0,0 +1,87 @@
+// Import relevant classes.
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+
+// Example SwerveDrive class
+public class SwerveDrive extends SubsystemBase
+{
+
+    // Attributes
+    SwerveDriveKinematics kinematics;
+    SwerveDriveOdometry   odometry;
+    Gyroscope             gyro; // Psuedo-class representing a gyroscope.
+    SwerveModule[]        swerveModules; // Psuedo-class representing swerve modules.
+
+    // Constructor
+    public SwerveDrive() 
+    {
+
+        swerveModules = new SwerveModule[4]; // Psuedo-code; Create swerve modules here.
+
+        // Create SwerveDriveKinematics object
+        // 12.5in from center of robot to center of wheel.
+        // 12.5in is converted to meters to work with object.
+        // Translation2d(x,y) == Translation2d(front, left)
+        kinematics = new SwerveDriveKinematics(
+            new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(12.5)), // Front Left
+            new Translation2d(Units.inchesToMeters(12.5), Units.inchesToMeters(-12.5)), // Front Right
+            new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(12.5)), // Back Left
+            new Translation2d(Units.inchesToMeters(-12.5), Units.inchesToMeters(-12.5))  // Back Right
+        );
+
+        gyro = new Gyroscope(); // Psuedo-constructor for generating gyroscope.
+
+        // Create the SwerveDriveOdometry given the current angle, the robot is at x=0, r=0, and heading=0
+        odometry = new SwerveDriveOdometry(
+            kinematics,
+            gyro.getAngle(), // returns current gyro reading as a Rotation2d
+            new SwerveModulePosition[]{new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition},
+            // Front-Left, Front-Right, Back-Left, Back-Right
+            new Pose2d(0,0,new Rotation2d()) // x=0, y=0, heading=0
+        );
+
+    }
+
+    // Simple drive function
+    public void drive()
+    {
+        // Create test ChassisSpeeds going X = 14in, Y=4in, and spins at 30deg per second.
+        ChassisSpeeds testSpeeds = new ChassisSpeeds(Units.inchesToMeters(14), Units.inchesToMeters(4), Units.degreesToRadians(30));
+
+        // Get the SwerveModuleStates for each module given the desired speeds.
+        SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(testSpeeds);
+        // Output order is Front-Left, Front-Right, Back-Left, Back-Right
+
+        swerveModules[0].setState(swerveModuleStates[0]);
+        swerveModules[1].setState(swerveModuleStates[1]);
+        swerveModules[2].setState(swerveModuleStates[2]);
+        swerveModules[3].setState(swerveModuleStates[3]);
+    }
+
+    // Fetch the current swerve module positions.
+    public SwerveModulePosition[] getCurrentSwerveModulePositions()
+    {
+        return new SwerveModulePosition[]{
+            new SwerveModulePosition(swerveModules[0].getDistance(), swerveModules[0].getAngle()), // Front-Left
+            new SwerveModulePosition(swerveModules[1].getDistance(), swerveModules[1].getAngle()), // Front-Right
+            new SwerveModulePosition(swerveModules[2].getDistance(), swerveModules[2].getAngle()), // Back-Left
+            new SwerveModulePosition(swerveModules[3].getDistance(), swerveModules[3].getAngle())  // Back-Right
+        };
+    }
+
+    @Override
+    public void periodic()
+    {
+        // Update the odometry every run.
+        odometry.update(gyro.getAngle(),  getCurrentSwerveModulePositions());
+    }
+
+}

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

@@ -0,0 +1,127 @@
+import com.revrobotics.CANSparkMax;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CANcoderConfigurator;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import edu.wpi.first.math.util.Units;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+
+
+public class SwerveModule {
+
+    private CANSparkMax driveMotor;
+    private CANSparkMax steerMotor;
+    private CANcoder    absoluteEncoder;
+    private SparkMaxPIDController drivingPIDController;
+    private SparkMaxPIDController turningPIDController;
+    private RelativeEncoder driveEncoder;
+    private RelativeEncoder steerEncoder;
+
+    public SwerveModule(int driveMotorCANID, int steerMotorCANID, int cancoderCANID)
+    {
+        driveMotor = new CANSparkMax(driveMotorCANID);
+        steerMotor = new CANSparkMax(steerMotorCANID);
+        absoluteEncoder = new CANcoder(cancoderCANID);
+
+        // Get the PID Controllers
+        drivingPIDController = driveMotor.getPIDController();
+        turningPIDController = steerMotor.getPIDController();
+
+        // Get the encoders
+        driveEncoder = driveMotor.getEncoder():
+        steerEncoder = steerMotor.getEncoder();
+
+        // Reset everything to factory default
+        driveMotor.restoreFactoryDefaults();
+        steerMotor.restoreFactoryDefaults();
+        absoluteEncoder.getConfigurator().apply(new CANcoderConfiguration());
+
+        // Continue configuration here..
+
+        // CANcoder Configuration
+        CANcoderConfigurator cfg = encoder.getConfigurator();
+        cfg.apply(new CANcoderConfiguration());
+        MagnetSensorConfigs  magnetSensorConfiguration = new MagnetSensorConfigs();
+        cfg.refresh(magnetSensorConfiguration);
+        cfg.apply(magnetSensorConfiguration
+                  .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+                  .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive));
+
+        // Steering Motor Configuration
+        steerMotor.setInverted(false);
+        turningPIDController.setFeedbackDevice(steerEncoder);
+        // Apply position and velocity conversion factors for the turning encoder. We
+        // want these in radians and radians per second to use with WPILib's swerve
+        // APIs.
+        steerEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor);
+        steerEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor);
+        // Enable PID wrap around for the turning motor. This will allow the PID
+        // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
+        // to 10 degrees will go through 0 rather than the other direction which is a
+        // longer route.
+        turningPIDController.setPositionPIDWrappingEnabled(true);
+        turningPIDController.setPositionPIDWrappingMinInput(0);
+        turningPIDController.setPositionPIDWrappingMaxInput(90);
+        // Set the PID gains for the turning motor. Note these are example gains, and you
+        // may need to tune them for your own robot!
+        turningPIDController.setP(ModuleConstants.kTurningP);
+        turningPIDController.setI(ModuleConstants.kTurningI);
+        turningPIDController.setD(ModuleConstants.kTurningD);
+        turningPIDController.setFF(ModuleConstants.kTurningFF);
+
+        // Drive Motor Configuration
+        driveMotor.setInverted(false);
+        drivingPIDController.setFeedbackDevice(driveEncoder);
+        // Apply position and velocity conversion factors for the driving encoder. The
+        // native units for position and velocity are rotations and RPM, respectively,
+        // but we want meters and meters per second to use with WPILib's swerve APIs.        
+        driveEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor);        
+        driveEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor);
+        // Set the PID gains for the driving motor. Note these are example gains, and you
+        // may need to tune them for your own robot!
+        drivingPIDController.setP(ModuleConstants.kDrivingP);
+        drivingPIDController.setI(ModuleConstants.kDrivingI);
+        drivingPIDController.setD(ModuleConstants.kDrivingD);
+        drivingPIDController.setFF(ModuleConstants.kDrivingFF);
+
+        // Save the SPARK MAX configurations. If a SPARK MAX browns out during
+        // operation, it will maintain the above configurations.
+        driveMotor.burnFlash();
+        steerMotor.burnFlash();
+
+        driveEncoder.setPosition(0);
+        steerEncoder.setPosition(encoder.getAbsolutePosition().refresh().getValue() * 360);
+    }
+
+
+    /**
+    Get the distance in meters.
+    */
+    public double getDistance()
+    {
+        return driveEncoder.getPosition();
+    }
+
+    /**
+    Get the angle.
+    */
+    public Rotation2d getAngle()
+    {
+          return Rotation2d.fromDegrees(steerEncoder.getPosition());
+    }
+
+    /**
+    Set the swerve module state.
+    @param state The swerve module state to set.
+    */
+    public void setState(SwerveModuleState state)
+    {
+          turningPIDController.setReference(state.angle.getDegrees(), ControlType.kPosition);
+          drivingPIDController.setReference(state.speedMetersPerSecond, ControlType.kVelocity);
+    }
+
+}