From a4984f8017589722012deaa4ed1b90e9b6075ab6 Mon Sep 17 00:00:00 2001
From: Keobkeig <angela.xue3@gmail.com>
Date: Thu, 4 Apr 2024 16:24:28 -0400
Subject: [PATCH] smooth drive

---
 .../com/stuypulse/robot/RobotContainer.java   | 14 +++++------
 .../commands/swerve/SwerveDriveDrive.java     | 25 ++++++++++++++-----
 2 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index 24229af..022578a 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -38,8 +38,8 @@ public class RobotContainer {
     // private final XboxController operator = new XboxController(OPERATOR);
     
     // Subsystem
-    private TankDrive drivetrain = TankDrive.getInstance();
-    //public SwerveDrive swerve = SwerveDrive.getInstance();
+    //private TankDrive drivetrain = TankDrive.getInstance();
+    public SwerveDrive swerve = SwerveDrive.getInstance();
     // private final Shooter shooter = Shooter.getInstance();
     //private Launcher launcher = Launcher.getInstance();
 
@@ -58,8 +58,8 @@ public RobotContainer() {
     /****************/
 
     private void configureDefaultCommands() {
-        drivetrain.setDefaultCommand(new TankDriveDrive(driver));
-        //swerve.setDefaultCommand(new SwerveDriveDrive(driver));
+        //drivetrain.setDefaultCommand(new TankDriveDrive(driver));
+        swerve.setDefaultCommand(new SwerveDriveDrive(driver));
         //launcher.setDefaultCommand(new LauncherHoldSpeed(1));
     }
 
@@ -83,9 +83,9 @@ private void configureButtonBindings() {
     public void configureAutons() {
         autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
 
-        // autonChooser.addOption("Preload Mobility", new PreloadMobility());
-        // autonChooser.addOption("Preload", new Preload());
-        //autonChooser.addOption("Mobility", new Mobility());
+        autonChooser.addOption("Preload Mobility", new PreloadMobility());
+        autonChooser.addOption("Preload", new Preload());
+        autonChooser.addOption("Mobility", new Mobility());
     
 
         SmartDashboard.putData("Autonomous", autonChooser);
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
index 3543084..51eb6f1 100644
--- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
+++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
@@ -3,6 +3,7 @@
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 
 import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -20,14 +21,18 @@ public SwerveDriveDrive(XboxController driver) {
     }
 
     public interface SwerveDriver {
-        double DRIVE_DEADBAND = 0.05; //"Driver Settings/Drive/Deadband"
-        double TURN_DEADBAND = 0.05; //"Driver Settings/Turn/Deadband"
+        // Low Pass Filter and deadband for Driver Controls
+        double DRIVE_DEADBAND = 0.05; //Driver Settings/Speed Deadband
+        double TURN_DEADBAND = 0.05; //Driver Settings/Turn Deadband
 
-        double DRIVE_RC = 0.05; //"Driver Settings/Turn/RC"
-        double TURN_RC = 0.05; //"Driver Settings/Turn/RC"
+        // Slew Rate Limiter Constants
+        double DRIVE_POWER = 2;
+        double DRIVE_SLEW_RATE = 8; //Driver Settings/Drive Slew Rate
+        double DRIVE_RC = 0.2; //Driver Settings/Drive RC Constant
 
-        double DRIVE_POWER = 2; //"Driver Settings/Turn/Power"
         double TURN_POWER = 1;
+        double TURN_SLEW_RATE = 10; //Driver Settings/Turn Slew Rate
+        double TURN_RC = 0.05; //Driver Settings/Drive RC Constant
 
         double MAX_TELOOP_SPEED = 5.55; //"Driver Settings/Drive/Max Speed"
         double MAX_TELEOP_TURNING = 6.0; //"Driver Settings/Turn/Max Turning"
@@ -38,6 +43,7 @@ public void execute() {
         swerve.drive(filterSpeed(new Translation2d(driver.getLeftX(), driver.getLeftY())), filterTurn(driver.getRightX())); 
     }
     
+    private final SlewRateLimiter speedSlew = new SlewRateLimiter(SwerveDriver.DRIVE_SLEW_RATE);
     private final LinearFilter speedFilter = LinearFilter.singlePoleIIR(SwerveDriver.DRIVE_RC, 0.02);
     public Translation2d filterSpeed(Translation2d speed) {
         if (speed.getNorm() < SwerveDriver.DRIVE_DEADBAND) {
@@ -50,6 +56,9 @@ public Translation2d filterSpeed(Translation2d speed) {
             xSpeed = Math.copySign(Math.pow(xSpeed, SwerveDriver.DRIVE_POWER), xSpeed);
             ySpeed = Math.copySign(Math.pow(ySpeed, SwerveDriver.DRIVE_POWER), ySpeed);
 
+            xSpeed = speedSlew.calculate(xSpeed);
+            ySpeed = speedSlew.calculate(ySpeed);
+
             xSpeed = speedFilter.calculate(xSpeed);
             ySpeed = speedFilter.calculate(ySpeed);
 
@@ -57,6 +66,7 @@ public Translation2d filterSpeed(Translation2d speed) {
         }        
     }
 
+    private final SlewRateLimiter turnSlew = new SlewRateLimiter(SwerveDriver.TURN_SLEW_RATE);
     private final LinearFilter turnFilter = LinearFilter.singlePoleIIR(SwerveDriver.TURN_RC, 0.02);
     public double filterTurn(double turn) {
         if (Math.abs(turn) < SwerveDriver.TURN_DEADBAND) {
@@ -64,7 +74,10 @@ public double filterTurn(double turn) {
         }
         else {
             turn = Math.copySign(Math.pow(turn, SwerveDriver.TURN_POWER), turn);
-            return turnFilter.calculate(turn);
+            turn = turnSlew.calculate(turn);
+            turn = turnFilter.calculate(turn);
+
+            return turn;
         }
     }
 }