Remove gyro feedback

StuyPulse · Jan 28, 2024 · b1e6b64 · b1e6b64
1 parent 03a238b
commit b1e6b64
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 39 deletions.
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
@@ -1,13 +1,10 @@
 package com.stuypulse.robot.commands.swerve;
 
-import java.util.Optional;
-
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Settings.Driver.Drive;
 import com.stuypulse.robot.constants.Settings.Driver.Turn;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.stuylib.input.Gamepad;
-import com.stuypulse.stuylib.math.Angle;
 import com.stuypulse.stuylib.math.SLMath;
 import com.stuypulse.stuylib.streams.numbers.IStream;
 import com.stuypulse.stuylib.streams.numbers.filters.RateLimit;
@@ -16,7 +13,6 @@
 import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
 import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class SwerveDriveDrive extends Command {
@@ -28,8 +24,6 @@ public class SwerveDriveDrive extends Command {
 
     private final Gamepad driver;
 
-    private Optional<Rotation2d> holdAngle;
-
     public SwerveDriveDrive(Gamepad driver) {
         this.driver = driver;
 
@@ -47,37 +41,17 @@ public SwerveDriveDrive(Gamepad driver) {
 
         turn = IStream.create(driver::getRightX)
             .filtered(
-          //      x -> SLMath.deadband(x, Turn.DEADBAND.get()),
-          //      x -> SLMath.spow(x, Turn.POWER.get()),
-                x -> x * Turn.MAX_TELEOP_TURNING.get()
-          //      new RateLimit(Turn.RC.get())
+                x -> SLMath.deadband(x, Turn.DEADBAND.get()),
+                x -> SLMath.spow(x, Turn.POWER.get()),
+                x -> x * Turn.MAX_TELEOP_TURNING.get(),
+                new RateLimit(Turn.RC.get())
         ); 
-
-        holdAngle = Optional.empty();
 
         addRequirements(swerve);
     }
 
-    @Override
-    public void initialize() {
-        holdAngle = Optional.empty();
-    }
-
-    private boolean isWithinTurnDeadband() {
-        return Math.abs(turn.get()) < Turn.DEADBAND.get();
-    }
-
     @Override
     public void execute() {
-        double angularVel = turn.get();
-
-        if(isWithinTurnDeadband() && holdAngle.isEmpty()){
-            holdAngle = Optional.of(swerve.getGyroAngle());
-        }
-        else {
-            holdAngle = Optional.empty(); 
-        }
-
-        swerve.drive(speed.get(), angularVel);
+        swerve.drive(speed.get(), turn.get());
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -198,14 +198,6 @@ public interface Turn {
             SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2);
 
             SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 6.0);
-
-            public interface GyroFeedback {
-                SmartBoolean GYRO_FEEDBACK_ENABLED = new SmartBoolean("Driver Settings/Gyro Feedback/Enabled", true);
-
-                SmartNumber P = new SmartNumber("Driver Settings/Gyro Feedback/kP", 0.5);
-                SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0);
-                SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1);
-            }
         }
     }