Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add filter on speed and turn in DriveTrainDrive #6

Open
wants to merge 2 commits into
base: newbie-ed/filters/main
Choose a base branch
from
Open

Add filter on speed and turn in DriveTrainDrive #6

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7593904
Add filter on speed and turn in DriveTrainDrive
BenG49 Feb 18, 2021
5395d25
Change
BenG49 Feb 18, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 31 additions & 1 deletion src/main/java/com/stuypulse/robot/commands/DrivetrainDriveCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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));

}
}