2023

README.md

@@ -1,14 +1,9 @@
 # SwerveBot-2022
-### Team 3512's swerve drive code for the 2022 offseason
-### Written in Java utilizing WPILib's Command-Based structure
-### Modified from Team 364's [BaseFalconSwerve](https://github.com/Team364/BaseFalconSwerve) project to utilize __NEO motors__ + __WPILib PID__ controllers.
+## Team 3512's swerve drive code for the 2022 offseason
+### Modified from Team 364's [BaseFalconSwerve](https://github.com/Team364/BaseFalconSwerve) project to utilize __NEO motors__
 
 Therefore, their [instructions](https://github.com/Team364/BaseFalconSwerve#setting-constants) on modifiying the constants to fit your robot will also work on this repo as well.
 
-### Note:
-Since it uses the WPILib PID controllers, this means that controls are running **onboard the roboRIO**. If you wish to utilize the __internal Spark Max PID controller__ to save utilization of the roboRIO, then modifications are needed.
-
 ### Configurations
 - Works with a swerve drive with __NEO brushless motors__, __Spark Max motor controllers__, __CTRE CANCoders__, and __CTRE Pigeon 2.0 IMU__.
-- Drive and steer ratios are configured for the __SDS MK4 L4 modules__, but can be easily be adapted to other modules.
-- We've also cut the speed by 50% in the [TeleopSwerve](https://github.com/frc3512/SwerveBot-2022/blob/main/src/main/java/frc/robot/commands/TeleopSwerve.java) command for our own robot. Definitely change/remove this accordingly, especially when using modules with different drive gear ratios.
+- Drive and steer ratios are configured for the __SDS MK4 L2 modules__, but can be easily be adapted to other modules. Remember to invert your turn motor on MK4Is.

src/main/java/frc/robot/CTREConfigs.java → src/main/java/frc/lib/config/CTREConfigs.java

@@ -1,9 +1,10 @@
-package frc.robot;
+package frc.lib.config;
 
 import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import com.ctre.phoenix.sensors.SensorInitializationStrategy;
 import com.ctre.phoenix.sensors.SensorTimeBase;
+import frc.robot.Constants;
 
 public final class CTREConfigs {
   public CANCoderConfiguration swerveCanCoderConfig;

src/main/java/frc/lib/util/SwerveModuleConstants.java → src/main/java/frc/lib/config/SwerveModuleConstants.java

@@ -1,10 +1,12 @@
-package frc.lib.util;
+package frc.lib.config;
+
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public class SwerveModuleConstants {
   public final int driveMotorID;
   public final int angleMotorID;
   public final int cancoderID;
-  public final double angleOffset;
+  public final Rotation2d angleOffset;
 
   /**
    * Swerve Module Constants to be used when creating swerve modules.
@@ -15,7 +17,7 @@ public class SwerveModuleConstants {
    * @param angleOffset
    */
   public SwerveModuleConstants(
-      int driveMotorID, int angleMotorID, int canCoderID, double angleOffset) {
+      int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {
     this.driveMotorID = driveMotorID;
     this.angleMotorID = angleMotorID;
     this.cancoderID = canCoderID;

src/main/java/frc/lib/util/CTREModuleState.java → src/main/java/frc/lib/math/OnboardModuleState.java

@@ -1,14 +1,15 @@
-package frc.lib.util;
+package frc.lib.math;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 
-public class CTREModuleState {
+public class OnboardModuleState {
 
   /**
    * Minimize the change in heading the desired swerve module state would require by potentially
    * reversing the direction the wheel spins. Customized from WPILib's version to include placing in
-   * appropriate scope for CTRE onboard control.
+   * appropriate scope for CTRE and REV onboard control as both controllers as of writing don't have
+   * support for continuous input.
    *
    * @param desiredState The desired state.
    * @param currentAngle The current module angle.

src/main/java/frc/lib/util/CANCoderUtil.java

@@ -0,0 +1,41 @@
+package frc.lib.util;
+
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderStatusFrame;
+
+/** Sets status frames for the CTRE CANCoder. */
+public class CANCoderUtil {
+  public enum CCUsage {
+    kAll,
+    kSensorDataOnly,
+    kFaultsOnly,
+    kMinimal
+  }
+
+  /**
+   * This function allows reducing a CANCoder's CAN bus utilization by reducing the periodic status
+   * frame period of nonessential frames from 10ms to 255ms.
+   *
+   * <p>See https://docs.ctre-phoenix.com/en/stable/ch18_CommonAPI.html#cancoder for a description
+   * of the status frames.
+   *
+   * @param cancoder The CANCoder to adjust the status frames on.
+   * @param usage The status frame feedback to enable. kAll is the default when a CANCoder
+   *     isconstructed.
+   */
+  public static void setCANCoderBusUsage(CANCoder cancoder, CCUsage usage) {
+    if (usage == CCUsage.kAll) {
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 10);
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.VbatAndFaults, 10);
+    } else if (usage == CCUsage.kSensorDataOnly) {
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 10);
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.VbatAndFaults, 100);
+    } else if (usage == CCUsage.kFaultsOnly) {
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 100);
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.VbatAndFaults, 10);
+    } else if (usage == CCUsage.kMinimal) {
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 100);
+      cancoder.setStatusFramePeriod(CANCoderStatusFrame.VbatAndFaults, 100);
+    }
+  }
+}

src/main/java/frc/robot/Constants.java

@@ -1,16 +1,18 @@
 package frc.robot;
 
 import com.revrobotics.CANSparkMax.IdleMode;
+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.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
-import frc.lib.util.SwerveModuleConstants;
+import frc.lib.config.SwerveModuleConstants;
 
 public final class Constants {
-  public static final double stickDeadband = 0.1;
 
   public static final class Swerve {
+    public static final double stickDeadband = 0.1;
+
     public static final int pigeonID = 6;
     public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
 
@@ -23,35 +25,46 @@ public static final class Swerve {
     public static final double openLoopRamp = 0.25;
     public static final double closedLoopRamp = 0.0;
 
-    public static final double driveGearRatio = (5.14 / 1.0); // 5.14:1
+    public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1
     public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1
 
     public static final SwerveDriveKinematics swerveKinematics =
         new SwerveDriveKinematics(
-            new Translation2d(wheelBase / 2.0, -trackWidth / 2.0),
             new Translation2d(wheelBase / 2.0, trackWidth / 2.0),
+            new Translation2d(wheelBase / 2.0, -trackWidth / 2.0),
             new Translation2d(-wheelBase / 2.0, trackWidth / 2.0),
             new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0));
 
+    /* Swerve Voltage Compensation */
+    public static final double voltageComp = 12.0;
+
     /* Swerve Current Limiting */
-    public static final int angleContinuousCurrentLimit = 25;
-    public static final int driveContinuousCurrentLimit = 35;
+    public static final int angleContinuousCurrentLimit = 20;
+    public static final int driveContinuousCurrentLimit = 80;
 
     /* Angle Motor PID Values */
-    public static final double angleKP = 0.99;
+    public static final double angleKP = 0.01;
     public static final double angleKI = 0.0;
     public static final double angleKD = 0.0;
+    public static final double angleKFF = 0.0;
 
     /* Drive Motor PID Values */
-    public static final double driveKP = 0.10;
+    public static final double driveKP = 0.1;
     public static final double driveKI = 0.0;
     public static final double driveKD = 0.0;
+    public static final double driveKFF = 0.0;
 
     /* Drive Motor Characterization Values */
     public static final double driveKS = 0.667;
     public static final double driveKV = 2.44;
     public static final double driveKA = 0.27;
 
+    /* Drive Motor Conversion Factors */
+    public static final double driveConversionPositionFactor =
+        (wheelDiameter * Math.PI) / driveGearRatio;
+    public static final double driveConversionVelocityFactor = driveConversionPositionFactor / 60.0;
+    public static final double angleConversionFactor = 360.0 / angleGearRatio;
+
     /* Swerve Profiling Values */
     public static final double maxSpeed = 4.5; // meters per second
     public static final double maxAngularVelocity = 11.5;
@@ -70,40 +83,40 @@ public static final class Swerve {
     /* Module Specific Constants */
     /* Front Left Module - Module 0 */
     public static final class Mod0 {
-      public static final int driveMotorID = 20;
-      public static final int angleMotorID = 10;
-      public static final int canCoderID = 7;
-      public static final double angleOffset = 144.22;
+      public static final int driveMotorID = 4;
+      public static final int angleMotorID = 3;
+      public static final int canCoderID = 1;
+      public static final Rotation2d angleOffset = Rotation2d.fromDegrees(327.48046875);
       public static final SwerveModuleConstants constants =
           new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
     }
 
     /* Front Right Module - Module 1 */
     public static final class Mod1 {
-      public static final int driveMotorID = 21;
-      public static final int angleMotorID = 11;
+      public static final int driveMotorID = 14;
+      public static final int angleMotorID = 13;
       public static final int canCoderID = 2;
-      public static final double angleOffset = 109.07;
+      public static final Rotation2d angleOffset = Rotation2d.fromDegrees(286.34765625);
       public static final SwerveModuleConstants constants =
           new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
     }
 
     /* Back Left Module - Module 2 */
     public static final class Mod2 {
-      public static final int driveMotorID = 12;
-      public static final int angleMotorID = 22;
+      public static final int driveMotorID = 2;
+      public static final int angleMotorID = 1;
       public static final int canCoderID = 3;
-      public static final double angleOffset = 326.07;
+      public static final Rotation2d angleOffset = Rotation2d.fromDegrees(55.01953125);
       public static final SwerveModuleConstants constants =
           new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
     }
 
     /* Back Right Module - Module 3 */
     public static final class Mod3 {
-      public static final int driveMotorID = 13;
-      public static final int angleMotorID = 23;
+      public static final int driveMotorID = 15;
+      public static final int angleMotorID = 16;
       public static final int canCoderID = 4;
-      public static final double angleOffset = 157.76;
+      public static final Rotation2d angleOffset = Rotation2d.fromDegrees(67.939453125);
       public static final SwerveModuleConstants constants =
           new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
     }

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.lib.config.CTREConfigs;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -16,9 +17,7 @@
  */
 public class Robot extends TimedRobot {
   public static CTREConfigs ctreConfigs;
-
   private Command m_autonomousCommand;
-
   private RobotContainer m_robotContainer;
 
   /**

src/main/java/frc/robot/RobotContainer.java

@@ -43,8 +43,8 @@ public RobotContainer() {
     s_Swerve.setDefaultCommand(
         new TeleopSwerve(
             s_Swerve,
-            () -> driver.getRawAxis(translationAxis),
-            () -> driver.getRawAxis(strafeAxis),
+            () -> -driver.getRawAxis(translationAxis),
+            () -> -driver.getRawAxis(strafeAxis),
             () -> -driver.getRawAxis(rotationAxis),
             () -> robotCentric.get()));
 

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

@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
@@ -15,6 +16,10 @@ public class TeleopSwerve extends CommandBase {
   private DoubleSupplier rotationSup;
   private BooleanSupplier robotCentricSup;
 
+  private SlewRateLimiter translationLimiter = new SlewRateLimiter(3.0);
+  private SlewRateLimiter strafeLimiter = new SlewRateLimiter(3.0);
+  private SlewRateLimiter rotationLimiter = new SlewRateLimiter(3.0);
+
   public TeleopSwerve(
       Swerve s_Swerve,
       DoubleSupplier translationSup,
@@ -34,11 +39,14 @@ public TeleopSwerve(
   public void execute() {
     /* Get Values, Deadband*/
     double translationVal =
-        MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband) * 0.5;
+        translationLimiter.calculate(
+            MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.Swerve.stickDeadband));
     double strafeVal =
-        MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband) * 0.5;
+        strafeLimiter.calculate(
+            MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.Swerve.stickDeadband));
     double rotationVal =
-        MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband) * 0.5;
+        rotationLimiter.calculate(
+            MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.Swerve.stickDeadband));
 
     /* Drive */
     s_Swerve.drive(