diff --git a/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java b/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java index ab43b3d..b0a434e 100644 --- a/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java @@ -2,6 +2,7 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.robot.subsystems.Drivetrain; + import edu.wpi.first.wpilibj2.command.CommandBase; // TODO: Find out what we can use from here @@ -13,12 +14,41 @@ public class DrivetrainDriveCommand extends CommandBase { private final Drivetrain drivetrain; private final Gamepad gamepad; + private IFilterGroup filterGroupSpeed; + private IFilterGroup filterGroupTurn; + public DrivetrainDriveCommand(Drivetrain drivetrain, Gamepad gamepad) { this.drivetrain = drivetrain; this.gamepad = gamepad; addRequirements(drivetrain); + + /* + We were bored + */ + filterGroupSpeed = new IFilterGroup( + new ExpMovingAverage(2), + // new HighPassFilter(0.21), + new LowPassFilter(3), + new MovingAverage(5), + new ExpMovingAverage(4), + new ExpMovingAverage(4), + new LowPassFilter(0.2), + new SpeedProfile(1), + new TimedMovingAverage(5) + ); + filterGroupTurn = new IFilterGroup( + new ExpMovingAverage(2), + // new HighPassFilter(0.21), + new LowPassFilter(3), + new MovingAverage(5), + new ExpMovingAverage(4), + new ExpMovingAverage(4), + new LowPassFilter(0.2), + new SpeedProfile(1), + new TimedMovingAverage(5) + ); } @Override @@ -29,7 +59,7 @@ public void execute() { // TODO: Filter these values sending them to the drivetrain - drivetrain.arcadeDrive(speed, turn); + drivetrain.arcadeDrive(filterGroupSpeed.get(speed), filterGroupTurn.get(turn)); } }