-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Transfers Joystick logic to seperate command * Updates DriveWithJoysticks to have BooleanSupplier * Update telemetry variable name
- Loading branch information
1 parent
77d4cda
commit 482a0bc
Showing
6 changed files
with
142 additions
and
71 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,84 @@ | ||
| package frc.robot.commands; | ||
|
|
||
| import java.util.function.BooleanSupplier; | ||
| import java.util.function.DoubleSupplier; | ||
|
|
||
| import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import frc.robot.CommandSwerveDrivetrain; | ||
| import frc.robot.Constants.DriveConstants; | ||
| import frc.robot.utils.Deadband; | ||
|
|
||
| public class DriveWithJoysticks extends Command { | ||
| CommandSwerveDrivetrain drivetrain; | ||
| DoubleSupplier x; | ||
| DoubleSupplier y; | ||
| DoubleSupplier rot; | ||
| BooleanSupplier fieldCentric; | ||
| BooleanSupplier babyMode; | ||
|
|
||
| double xMpS; | ||
| double yMpS; | ||
| double rotRadpS; | ||
|
|
||
| SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric().withIsOpenLoop(true); | ||
| SwerveRequest.RobotCentric driveRobot = new SwerveRequest.RobotCentric().withIsOpenLoop(true); | ||
| SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); | ||
| SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); | ||
|
|
||
| public DriveWithJoysticks(CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y, | ||
| DoubleSupplier rot, BooleanSupplier fieldCentric, BooleanSupplier babyMode) { | ||
| this.drivetrain = drivetrain; | ||
| this.x = x; | ||
| this.y = y; | ||
| this.rot = rot; | ||
| this.fieldCentric = fieldCentric; | ||
| this.babyMode = babyMode; | ||
|
|
||
| addRequirements(drivetrain); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| double[] joystickInputsFiltered = Deadband.twoAxisDeadband(x.getAsDouble(), y.getAsDouble(), | ||
| DriveConstants.deadbandPercent); | ||
|
|
||
| xMpS = joystickInputsFiltered[0] * DriveConstants.MaxSpeedMetPerSec; | ||
| yMpS = joystickInputsFiltered[1] * DriveConstants.MaxSpeedMetPerSec; | ||
| rotRadpS = Deadband.oneAxisDeadband(rot.getAsDouble(), DriveConstants.deadbandPercent) | ||
| * DriveConstants.MaxAngularRateRadiansPerSec; | ||
|
|
||
| if (babyMode.getAsBoolean()) { | ||
| xMpS *= 0.5; | ||
| yMpS *= 0.5; | ||
| rotRadpS *= 0.5; | ||
| } | ||
|
|
||
| if (xMpS == 0 && yMpS == 0 && rotRadpS == 0) { | ||
| // If not moving, brake | ||
| drivetrain.applyRequest(() -> brake); | ||
| } else if (fieldCentric.getAsBoolean()) { | ||
| drivetrain.applyRequest(() -> drive.withVelocityX(-xMpS) | ||
| .withVelocityY(-yMpS) | ||
| .withRotationalRate(-rotRadpS)); | ||
| } else { | ||
| drivetrain.applyRequest(() -> driveRobot.withVelocityX(-xMpS) | ||
| .withVelocityY(-yMpS) | ||
| .withRotationalRate(-rotRadpS)); | ||
| } | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return false; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,40 @@ | ||
| package frc.robot.utils; | ||
|
|
||
| public class Deadband { | ||
| // 1D Deadband in linear format | ||
| public static double oneAxisDeadband(double input, double deadband) { | ||
| if (Math.abs(input) < deadband) { | ||
| return 0; | ||
| } else { | ||
| return input; | ||
| } | ||
| } | ||
|
|
||
| // 2D Deadband in a circular format | ||
| public static double[] twoAxisDeadband(double inputX, double inputY, double deadband) { | ||
| double[] output = new double[2]; | ||
| if (Math.sqrt(Math.pow(inputX, 2) + Math.pow(inputY, 2)) < deadband) { | ||
| output[0] = 0; | ||
| output[1] = 0; | ||
| } else { | ||
| output[0] = inputX; | ||
| output[1] = inputY; | ||
| } | ||
| return output; | ||
| } | ||
|
|
||
| // 3D Deadband in a spherical format | ||
| public static double[] threeAxisDeadband(double inputX, double inputY, double inputZ, double deadband) { | ||
| double[] output = new double[3]; | ||
| if (Math.sqrt(Math.pow(inputX, 2) + Math.pow(inputY, 2) + Math.pow(inputZ, 2)) < deadband) { | ||
| output[0] = 0; | ||
| output[1] = 0; | ||
| output[2] = 0; | ||
| } else { | ||
| output[0] = inputX; | ||
| output[1] = inputY; | ||
| output[2] = inputZ; | ||
| } | ||
| return output; | ||
| } | ||
| } |