Fix tankdrivedrive

Co-authored-by: Zixi Feng <[email protected]>
Co-authored-by: Richie Xue <[email protected]>

src/main/java/com/stuypulse/robot/commands/auton/PreloadMobility.java

@@ -1,6 +1,6 @@
 package com.stuypulse.robot.commands.auton;
 
-import com.stuypulse.robot.commands.shooter.ShootCommand;
+// import com.stuypulse.robot.commands.shooter.ShootCommand;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 

src/main/java/com/stuypulse/robot/commands/tank/TankDriveDrive.java

@@ -15,20 +15,26 @@ public class TankDriveDrive extends Command {
     private TankDrive tank;
     private XboxController driver;
 
+    private final SlewRateLimiter driveLimiter;
+    private final SlewRateLimiter turnLimiter;
+
     double speed;
     double turn;
 
     public TankDriveDrive(TankDrive TankDrive, XboxController driver) {
         this.tank = TankDrive;
         this.driver = driver;
 
-        speed = filterSpeed(driver.getLeftX());
-        turn = filterTurn(driver.getRightX()); 
+        driveLimiter = new SlewRateLimiter(0.5);
+        turnLimiter = new SlewRateLimiter(0.5);
+
+        speed = 0;
+        turn = 0; 
 
         addRequirements(TankDrive);
     }
 
-     public interface Drivetrain {
+    public interface Drivetrain {
         // If speed is below this, use quick turn
         double BASE_TURNING_SPEED = 0.45; //Driver Settings/Base Turn Speed
 
@@ -81,41 +87,32 @@ public interface PID {
         }
     }
 
-    public double filterSpeed(double speed) {
-
-        SlewRateLimiter filter = new SlewRateLimiter(0.5); //limits controller inputs to 0.5units/secondMath.abs(speed)   Drivetrain.SPEED_DEADBAND) {Turn.DEADBAND ) {
+    public double filterSpeed(double speed) {        
         if (Math.abs(speed) < Drivetrain.SPEED_DEADBAND) {
             return 0; 
-        }
-        else {
-
-            return SLpow(filter.calculate(speed), Drivetrain.SPEED_POWER);
+        } else {
+            return pow(driveLimiter.calculate(speed), Drivetrain.SPEED_POWER);
         }  
     }
 
     public double filterTurn(double turn) {
-        SlewRateLimiter filter = new SlewRateLimiter(0.5);
-
         if (Math.abs(turn) < Turn.DEADBAND) {
             return 0;
-        }
-        else {
-            return SLpow(filter.calculate(turn), Drivetrain.ANGLE_POWER);
+        } else {
+            return pow(turnLimiter.calculate(turn), Drivetrain.ANGLE_POWER);
         }
     }
 
-    public double SLpow(double value, double expo){
-        if (value < 0) {
-            return Math.pow(value, expo) * -1.0;
-        }
-        else {
-            return Math.pow(value, expo);
-        }
+    public double pow(double value, double expo){
+        if (value < 0) return Math.pow(value, expo) * -1.0;
+        else           return Math.pow(value, expo);
     }
 
     @Override
     public void execute() {
-        tank.curvatureDrive(speed, turn);
+        tank.curvatureDrive(
+            filterSpeed(driver.getLeftX()),
+            filterTurn(driver.getRightX()));
     }
 
     @Override