Working preload auto and clearer TankDriveDrive

Co-authored-by: Ian Shi <IanShiii@users.noreply.github.com>

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -83,11 +83,10 @@ private void configureButtonBindings() {
     public void configureAutons() {
         autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
 
-        if (SwerveDrive.instance != null) {
-            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);
     }

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

@@ -1,15 +1,18 @@
 package com.stuypulse.robot.commands.auton;
 
 import com.stuypulse.robot.commands.swerve.SwerveDriveBack;
+import com.stuypulse.robot.commands.tank.TankDriveBack;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 public class Mobility extends SequentialCommandGroup {
     public Mobility() {
         addCommands(
-            new WaitCommand(8),
-            new SwerveDriveBack(2).withTimeout(2)
+            new WaitCommand(2),
+
+            // new SwerveDriveBack(2).withTimeout(2)
+            new TankDriveBack().withTimeout(2)
         );
     }
-}
+}

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

@@ -1,5 +1,6 @@
 package com.stuypulse.robot.commands.auton;
 
+import com.stuypulse.robot.commands.launcher.LauncherLaunch;
 import com.stuypulse.robot.commands.shooter.ShootCommand;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -9,7 +10,9 @@ public class Preload extends SequentialCommandGroup {
     public Preload() {
         addCommands(
             new WaitCommand(2),
-            new ShootCommand(1,1)
+            new LauncherLaunch(0,1).withTimeout(0.25),
+            new LauncherLaunch(1, 1)
+            // new ShootCommand(1,1)
         );
     }
 }

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

@@ -0,0 +1,29 @@
+package com.stuypulse.robot.commands.tank;
+
+import com.stuypulse.robot.subsystems.TankDrive;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class TankDriveBack extends Command {
+    private TankDrive tank;
+
+    public TankDriveBack() {
+        this.tank = TankDrive.getInstance();
+        addRequirements(tank);
+    }
+
+    @Override
+    public void initialize() {
+        tank.tankDriveVolts(-12,-12);
+    }
+
+    @Override
+    public void execute() {
+        tank.tankDriveVolts(-12,-12);
+    }
+
+    @Override 
+    public void end(boolean isInterrupted) {
+        tank.stop();
+    }
+}

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

@@ -2,6 +2,7 @@
 
 import com.stuypulse.robot.subsystems.TankDrive;
 
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -10,48 +11,51 @@ public class TankDriveDrive extends Command {
 
     private TankDrive tank;
     private XboxController driver;
-
-    private final SlewRateLimiter driveSlew;
-    private final SlewRateLimiter turnSlew;
-
-    double speed;
-    double turn;
 
     public TankDriveDrive(XboxController driver) {
         this.tank = TankDrive.getInstance();
         this.driver = driver;
-
-        driveSlew = new SlewRateLimiter(Drivetrain.DRIVE_SLEW_RATE);
-        turnSlew = new SlewRateLimiter(Drivetrain.TURN_SLEW_RATE);
-
         addRequirements(tank);
     }
 
     public interface Drivetrain {
         // Low Pass Filter and deadband for Driver Controls
-        double SPEED_DEADBAND = 0.02; //Driver Settings/Speed Deadband
+        double SPEED_DEADBAND = 0.05; //Driver Settings/Speed Deadband
         double ANGLE_DEADBAND = 0.05; //Driver Settings/Turn Deadband
 
         // Slew Rate Limiter Constants
-        double DRIVE_SLEW_RATE = 8; //Driver Settings/Drive Slew Rate
-        double TURN_SLEW_RATE = 16; //Driver Settings/Turn Slew Rate
+        double DRIVE_POWER = 2;
+        double DRIVE_SLEW_RATE = 5; //Driver Settings/Drive Slew Rate
+        double DRIVE_RC_CONSTANT = 0.1; //Driver Settings/Drive RC Constant
+
+        double TURN_POWER = 1;
+        double TURN_SLEW_RATE = 10; //Driver Settings/Turn Slew Rate
+        double TURN_RC_CONSTANT = 0.05; //Driver Settings/Drive RC Constant
     }
 
-    public double filterSpeed(double newSpeed) {
-        newSpeed = spow(newSpeed, 2); 
-        newSpeed = driveSlew.calculate(newSpeed);    
-        speed += (newSpeed - speed) * 0.75;   
+    private final SlewRateLimiter speedSlewRate = new SlewRateLimiter(Drivetrain.DRIVE_SLEW_RATE);
+    private final LinearFilter speedLinearFilter = LinearFilter.singlePoleIIR(Drivetrain.DRIVE_SLEW_RATE, 0.02);
+
+    public double filterSpeed(double speed) {
+        speed = spow(speed, Drivetrain.DRIVE_POWER);
+        speed = speedSlewRate.calculate(speed);
+        speed = speedLinearFilter.calculate(speed);
+
         if (Math.abs(speed) < Drivetrain.SPEED_DEADBAND) {
             return 0; 
         } else {
             return speed;
         }  
     }
 
-    public double filterTurn(double newTurn) {
-        newTurn = spow(newTurn, 1);
-        newTurn = turnSlew.calculate(newTurn);
-        turn += (newTurn - turn) * 0.75;  
+    private final SlewRateLimiter turnSlew = new SlewRateLimiter(Drivetrain.TURN_SLEW_RATE);
+    private final LinearFilter turnLinearFilter = LinearFilter.singlePoleIIR(Drivetrain.TURN_SLEW_RATE, 0.02);
+
+    public double filterTurn(double turn) {
+        turn = spow(turn, Drivetrain.TURN_POWER);
+        turn = turnSlew.calculate(turn);
+        turn = turnLinearFilter.calculate(turn); 
+
         if (Math.abs(turn) < Drivetrain.ANGLE_DEADBAND) {
             return 0;
         } else {

src/main/java/com/stuypulse/robot/subsystems/TankDrive.java

@@ -150,7 +150,7 @@ public double getVelocity() {
 
     /***************
      * ROBOT ANGLE *
-     ***************/
+    ***************/
 
     //Gets current Angle of the Robot as a double (contiuous / not +-180)
     public Rotation2d getRawGyroAngle() {
@@ -186,18 +186,22 @@ public void stop() {
     }
 
     public void tankDrive(double left, double right) {
-        tankDrive.tankDrive(left, right, true);
+        tankDrive.tankDrive(left, right, false);
     }
 
     public void arcadeDrive(double speed, double rotation) {
-        tankDrive.arcadeDrive(speed, rotation, true);
+        tankDrive.arcadeDrive(speed, rotation, false);
     }
 
     public void curvatureDrive(double xSpeed, double zRotation) {
         tankDrive.curvatureDrive(xSpeed, zRotation * Math.max(Math.abs(xSpeed), 0.33), true);
     }
 
-
+    public void tankDriveVolts(double leftVolts, double rightVolts) {
+        leftMotors[0].setVoltage(rightVolts);
+        rightMotors[0].setVoltage(rightVolts);
+        tankDrive.feed();
+    }
     /**********************
      * ODOMETRY FUNCTIONS *
      **********************/
@@ -250,6 +254,8 @@ public void periodic() {
         updateOdometry();
         field.setRobotPose(getPose());
 
+        tankDrive.feed();
+
         // Smart Dashboard Information
         SmartDashboard.putNumber("TankDrive/Roll (deg)", getRoll().getDegrees());