Joystick control (#5) Swerve drives.

Co-authored-by: WispySparks <[email protected]>

.gitignore

@@ -1,6 +1,10 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 
+networktables.json
+simgui-ds.json
+simgui.json
+
 ### C++ ###
 # Prerequisites
 *.d

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

@@ -23,5 +23,5 @@
     "drive": false,
     "angle": false
   },
-  "absoluteEncoderInverted": false
+  "absoluteEncoderInverted": true
 }

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

@@ -7,9 +7,9 @@
     "iz": 0
   },
   "angle": {
-    "p": 0,
+    "p": 0.75,
     "i": 0,
-    "d": 0,
+    "d": 0.55, 
     "f": 0,
     "iz": 0
   }

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

@@ -14,22 +14,32 @@
 import edu.wpi.first.wpilibj.Filesystem;
 
 public class Constants {
-    public static class SwerveK {
-        public static final int driveMotorID = 0;
-        public static final int turnMotorID = 1;
-        public static final int encoderID = 2;
 
+    public static class SwerveK {
         public static final Measure<Distance> wheelDiameter = Inches.of(3); 
         public static final Measure<Distance> driveBaseRadius = Meters.of(0.4579874);
 
-        public static final double steerGearRatio = 12.8; //! Figure Out;
-        public static final double driveGearRatio = 6; //! Figure Out
+        public static final double steerGearRatio = 1;
+        public static final double driveGearRatio = 6;
 
-        public static final Measure<Velocity<Distance>> maxModuleSpeed = MetersPerSecond.of(0); //! Tune
+        public static final Measure<Velocity<Distance>> maxRobotSpeed = MetersPerSecond.of(4.24);
 
-        public static final PIDConstants translationConstants = new PIDConstants(1, 1, 1); //! Tune
-        public static final PIDConstants rotationConstants = new PIDConstants(1, 1, 1); //! Tune
+        public static final PIDConstants translationConstants = new PIDConstants(1, 1, 1); //! TODO: Tune
+        public static final PIDConstants rotationConstants = new PIDConstants(1, 1, 1); //! TODO: Tune
 
         public static final File swerveDirectory = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/swerve");
     }
+
+    public static class ControllerK {
+        public static final int xboxPort = 0;
+        public static final double leftJoystickDeadband = 0.05;
+        public static final double rightJoystickDeadband = 0.05;
+    }
+
+    public static class DriveK {
+        public static final DynamicSlewRateLimiter translationalYLimiter = new DynamicSlewRateLimiter(0.5, 2); // Larger number = faster rate of change
+        public static final DynamicSlewRateLimiter translationalXLimiter = new DynamicSlewRateLimiter(0.5, 2);
+        public static final DynamicSlewRateLimiter rotationalLimiter = new DynamicSlewRateLimiter(0.5, 2);
+    }
+
 }

src/main/java/frc/robot/DynamicSlewRateLimiter.java

@@ -0,0 +1,81 @@
+package frc.robot;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUtil;
+
+/**
+* A class that limits the rate of change of an input value. Can have different rate limits for 
+* getting farther from zero (increasing) and getting closer to zero (decreasing). Useful for 
+* controlling something like robot acceleration and deceleration.
+*/
+public class DynamicSlewRateLimiter {
+    private final double increasingRateLimit;
+    private final double decreasingRateLimit;
+    private double prevVal;
+    private double prevTime;
+
+    /**
+    * Creates a new DynamicSlewRateLimiter with the given increasing and decreasing rate limits.
+    * Increasing is how fast the input can get farther from zero, Decreasing is how fast the input can get closer to zero.
+    * The rate limits are only magnitudes.
+    * @param increasingRateLimit The rate-of-change limit when the input is increasing, in units per
+    *     second. This is expected to be positive. How quickly the input can get farther from zero.
+    * @param decreasingRateLimit The rate-of-change limit when the input is decreasing, in units per
+    *     second. This is expected to be positive. How quickly the input can get closer to zero.
+    */
+    public DynamicSlewRateLimiter(double increasingRateLimit, double decreasingRateLimit) {
+        if (increasingRateLimit < 0 || decreasingRateLimit < 0) {
+            throw new IllegalArgumentException("Rate limits can't be negative!");
+        }
+        this.increasingRateLimit = increasingRateLimit;
+        this.decreasingRateLimit = decreasingRateLimit;
+        prevVal = 0;
+        prevTime = MathSharedStore.getTimestamp();
+    }
+
+    /**
+    * Filters the input to limit its slew rate.
+    *
+    * @param input The input value whose slew rate is to be limited.
+    * @return The filtered value, which will not change faster than the slew rate.
+    */
+    public double calculate(double input) {
+        double currentTime = MathSharedStore.getTimestamp();
+        double elapsedTime = currentTime - prevTime;
+        double sign = Math.signum(prevVal);
+        double positiveRateLimit = increasingRateLimit;
+        double negativeRateLimit = decreasingRateLimit;
+        // Flip the positive and negative limits so that decreasing still means towards zero and increasing still means away.
+        if (sign < 0) { 
+            positiveRateLimit = decreasingRateLimit;
+            negativeRateLimit = increasingRateLimit;
+        } 
+        prevVal +=
+        MathUtil.clamp(
+            input - prevVal,
+            -negativeRateLimit * elapsedTime,
+            positiveRateLimit * elapsedTime);
+        prevTime = currentTime;
+        return prevVal;
+    }
+
+    /**
+    * Returns the value last calculated by the DynamicSlewRateLimiter.
+    *
+    * @return The last value.
+    */
+    public double lastValue() {
+        return prevVal;
+    }
+
+    /**
+    * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+    *
+    * @param value The value to reset to.
+    */
+    public void reset(double value) {
+        prevVal = value;
+        prevTime = MathSharedStore.getTimestamp();
+    }
+
+}

src/main/java/frc/robot/Field.java

@@ -1,4 +1,5 @@
 package frc.robot;
+
 public class Field {
 
 }

src/main/java/frc/robot/Main.java

@@ -7,9 +7,11 @@
 import edu.wpi.first.wpilibj.RobotBase;
 
 public final class Main {
+
   private Main() {}
 
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);
   }
+
 }

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

@@ -4,16 +4,43 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.ControllerK;
+import frc.robot.Constants.DriveK;
 import frc.robot.subsystems.Swerve;
 
 public class Robot extends TimedRobot {
 
     private final Swerve swerve = new Swerve();
+    private final CommandXboxController xboxController = new CommandXboxController(ControllerK.xboxPort);
 
     public Robot() {
-
+        DriverStation.silenceJoystickConnectionWarning(true);
+        addPeriodic(() -> CommandScheduler.getInstance().run(), kDefaultPeriod);
+        configureBindings();
+        Command driveFieldOrientedAngularVelocity = swerve.driveCommand(
+            () -> DriveK.translationalYLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftY(), ControllerK.leftJoystickDeadband)), 
+            () -> DriveK.translationalXLimiter.calculate(MathUtil.applyDeadband(-xboxController.getLeftX(), ControllerK.leftJoystickDeadband)),  
+            () -> DriveK.rotationalLimiter.calculate(MathUtil.applyDeadband(-xboxController.getRightX(), ControllerK.rightJoystickDeadband)),
+            false
+        );
+
+        swerve.setDefaultCommand(driveFieldOrientedAngularVelocity);
+    }
+
+    private void configureBindings() {
+        // Reset forward direction for field relative
+        xboxController.x().and(xboxController.b()).onTrue(swerve.runOnce(swerve::zeroGyro)); 
     }
 
-}
+    @Override
+    public void robotPeriodic() {
 
+    }
+
+}