From 75939044fa37d3e7de7b7e972b5597ec8d7b9a16 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 18 Feb 2021 17:28:37 -0500 Subject: [PATCH 1/2] Add filter on speed and turn in DriveTrainDrive --- .../robot/commands/DrivetrainDriveCommand.java | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java b/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java index ab43b3d..c2ebabd 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,26 @@ public class DrivetrainDriveCommand extends CommandBase { private final Drivetrain drivetrain; private final Gamepad gamepad; + private IFilterGroup filterGroup; + public DrivetrainDriveCommand(Drivetrain drivetrain, Gamepad gamepad) { this.drivetrain = drivetrain; this.gamepad = gamepad; addRequirements(drivetrain); + + filterGroup = new IFilterGroup(new IFilter[] { + 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 +44,7 @@ public void execute() { // TODO: Filter these values sending them to the drivetrain - drivetrain.arcadeDrive(speed, turn); + drivetrain.arcadeDrive(filterGroup.get(speed), filterGroup.get(turn)); } } From 5395d25b8256cbb767d90d901fbb70600ae23135 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 18 Feb 2021 17:38:26 -0500 Subject: [PATCH 2/2] Change --- .../commands/DrivetrainDriveCommand.java | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java b/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java index c2ebabd..b0a434e 100644 --- a/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java @@ -14,7 +14,8 @@ public class DrivetrainDriveCommand extends CommandBase { private final Drivetrain drivetrain; private final Gamepad gamepad; - private IFilterGroup filterGroup; + private IFilterGroup filterGroupSpeed; + private IFilterGroup filterGroupTurn; public DrivetrainDriveCommand(Drivetrain drivetrain, Gamepad gamepad) { @@ -23,9 +24,12 @@ public DrivetrainDriveCommand(Drivetrain drivetrain, Gamepad gamepad) { addRequirements(drivetrain); - filterGroup = new IFilterGroup(new IFilter[] { + /* + We were bored + */ + filterGroupSpeed = new IFilterGroup( new ExpMovingAverage(2), - new HighPassFilter(0.21), + // new HighPassFilter(0.21), new LowPassFilter(3), new MovingAverage(5), new ExpMovingAverage(4), @@ -33,7 +37,18 @@ public DrivetrainDriveCommand(Drivetrain drivetrain, Gamepad gamepad) { 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 @@ -44,7 +59,7 @@ public void execute() { // TODO: Filter these values sending them to the drivetrain - drivetrain.arcadeDrive(filterGroup.get(speed), filterGroup.get(turn)); + drivetrain.arcadeDrive(filterGroupSpeed.get(speed), filterGroupTurn.get(turn)); } }