Version 2.1.0: Add acceleration limiting

Author: Benji123abc

build.gradle

@@ -14,7 +14,7 @@ plugins {
 }
 
 group 'frc.team4373'
-version '2.0.1'
+version '2.1.0'
 
 sourceCompatibility = 1.11
 

src/main/java/frc/team4373/swerve/SwerveDrivetrain.java

@@ -291,6 +291,18 @@ public BrakeMode getBrakeMode() {
         return this.brakeMode;
     }
 
+
+    /**
+     * Sets the max acceleration for each wheel.
+     * @param maxAccel the maximum acceleration.
+     */
+    public void setMaxWheelAcceleration(double maxAccel) {
+        right1.setMaxWheelAcceleration(maxAccel);
+        right2.setMaxWheelAcceleration(maxAccel);
+        left1.setMaxWheelAcceleration(maxAccel);
+        left2.setMaxWheelAcceleration(maxAccel);
+    }
+
     /**
      * Gets the swerve wheel with the specified ID.
      * @param wheelID the ID of the wheel to fetch.

src/main/java/frc/team4373/swerve/SwerveWheel.java

@@ -17,6 +17,10 @@ class SwerveWheel {
 
     private boolean isInverted = false;
 
+    private double maxAcceleration = 2;
+
+    private double lastSetSpeed = 0;
+
     /**
      * Constructs a new swerve wheel for the specified wheel.
      * @param driveMotorConfig the config for the drive motor.
@@ -90,6 +94,16 @@ class SwerveWheel {
      */
     void set(double speed, double heading) {
 
+        double acceleration = speed - lastSetSpeed;
+
+        if (acceleration > maxAcceleration) {
+            speed = lastSetSpeed + maxAcceleration;
+        } else if (acceleration < -maxAcceleration) {
+            speed = lastSetSpeed - maxAcceleration;
+        }
+
+        lastSetSpeed = speed;
+
         heading *= SwerveConstants.DEGREES_TO_ENCODER_UNITS;
 
         double currentRotation = rotatorMotor.getSelectedSensorPosition();
@@ -235,4 +249,8 @@ void resetAbsoluteEncoder() {
     double getRotatorMotorVelocity() {
         return rotatorMotor.getSelectedSensorVelocity();
     }
+
+    void setMaxWheelAcceleration(double maxAccel) {
+        maxAcceleration = maxAccel;
+    }
 }