Skip to content

Commit

Permalink
Merge pull request #62 from frc937/suppliers
Browse files Browse the repository at this point in the history 
Add drive command suppliers
  • Loading branch information
@quackitsquinn
quackitsquinn authored Feb 20, 2024
2 parents 655c4c3 + d81197e commit f08d0ac
Show file tree
Hide file tree
Showing 4 changed files with 102 additions and 29 deletions.
32 changes: 30 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -37,6 +38,7 @@
import frc.robot.subsystems.mailbox.Mailbox;
import frc.robot.subsystems.mailbox.MailboxBelts;
import frc.robot.subsystems.mailbox.MailboxPneumatics;
import java.util.function.Supplier;

@SuppressWarnings("unused")
/** Singleton class that contains all the robot's subsystems, commands, and button bindings. */
Expand Down Expand Up @@ -74,14 +76,38 @@ public class RobotContainer {
/** Singleton instance of {@link UrMom} for the whole robot. */
public static UrMom urMom = new UrMom();

/*
* *************
* * SUPPLIERS *
* *************
*/
private Supplier<Double> scaledControllerLeftXAxisSupplier = () -> getScaledControllerLeftXAxis();
private Supplier<Double> scaledControllerLeftYAxisSupplier = () -> getScaledControllerLeftYAxis();
private Supplier<Double> scaledControllerRightXAxisSupplier =
() -> getScaledControllerRightXAxis();
private Supplier<Double> scaledControllerRightYAxisSupplier =
() -> getScaledControllerRightYAxis();
private Supplier<Boolean> povUpDirectionSupplier = () -> rawXboxController.getPOV() == 0;
private Supplier<Boolean> povRightDirectionSupplier = () -> rawXboxController.getPOV() == 90;
private Supplier<Boolean> povDownDirectionSupplier = () -> rawXboxController.getPOV() == 180;
private Supplier<Boolean> povLeftDirectionSupplier = () -> rawXboxController.getPOV() == 270;

/*
* ************
* * COMMANDS *
* ************
*/

private DriveRobotOriented driveRobotOriented = new DriveRobotOriented();
private DriveFieldOriented driveFieldOriented = new DriveFieldOriented();
private DriveRobotOriented driveRobotOriented =
new DriveRobotOriented(
scaledControllerLeftYAxisSupplier,
scaledControllerLeftXAxisSupplier,
scaledControllerRightXAxisSupplier);
private DriveFieldOriented driveFieldOriented =
new DriveFieldOriented(
scaledControllerLeftXAxisSupplier,
scaledControllerLeftYAxisSupplier,
scaledControllerRightXAxisSupplier);
private EnterXMode enterXMode = new EnterXMode();
private DeployPneumatics deployPneumatics = new DeployPneumatics();
private RunBelts runBelts = new RunBelts();
Expand Down Expand Up @@ -121,6 +147,8 @@ public class RobotContainer {
public static CommandXboxController driverController =
new CommandXboxController(Constants.Controllers.DRIVER_CONTROLLER_PORT);

private static XboxController rawXboxController = driverController.getHID();

/** Sendable Chooser for autos. */
private SendableChooser<Command> autoChooser;

Expand Down
22 changes: 17 additions & 5 deletions src/main/java/frc/robot/commands/drive/DriveFieldOriented.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,13 +16,25 @@
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.subsystems.Drive;
import java.util.function.Supplier;

/** Drives the robot in field-oriented mode. */
public class DriveFieldOriented extends Command {
private final Drive drive;
private final Supplier<Double> xSupplier, ySupplier, zSupplier;

/** Creates a new DriveFieldOriented. */
public DriveFieldOriented() {
/**
* Drives the robot field-oriented
*
* @param xSupplier The joystick value for the Y axis. [-1, 1] left positive.
* @param ySupplier The joystick value for the Y axis. [-1, 1] back positive.
* @param zSupplier The joystick value for the Z axis. [-1, 1] counterclockwise positive.
*/
public DriveFieldOriented(
Supplier<Double> xSupplier, Supplier<Double> ySupplier, Supplier<Double> zSupplier) {
this.xSupplier = xSupplier;
this.ySupplier = ySupplier;
this.zSupplier = zSupplier;
this.drive = RobotContainer.drive;
addRequirements(drive);
}
Expand All @@ -34,9 +46,9 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED;
double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED;
double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED;
double x = this.xSupplier.get() * Constants.Drive.MAX_SPEED;
double y = this.ySupplier.get() * Constants.Drive.MAX_SPEED;
double z = this.zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED;
Translation2d translation = new Translation2d(x, y);

drive.driveFieldOriented(translation, z);
Expand Down
55 changes: 38 additions & 17 deletions src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.subsystems.Drive;
import java.util.function.Supplier;
import swervelib.SwerveController;

/**
Expand All @@ -27,11 +28,39 @@
public class DriveFieldOrientedHeadingSnapping extends Command {
private Drive drive;
private XboxController controller;
private final Supplier<Double> xSupplier, ySupplier, zSupplier;
private final Supplier<Boolean> upSupplier, downSupplier, leftSupplier, rightSupplier;

/** Creates a new DriveFieldOrientedHeadingSnapping. */
public DriveFieldOrientedHeadingSnapping() {
/**
* Drives the robot field oriented with heading snapping. All values are suppliers to make the
* command more versatile.
*
* @param xSupplier The joystick value for the Y axis. [-1, 1] left positive.
* @param ySupplier The joystick value for the Y axis. [-1, 1] back positive.
* @param zSupplier The joystick value for the Z axis. [-1, 1] counterclockwise positive.
* @param upSupplier If true, the robot heading will be set to move towards the opposing alliance
* wall
* @param downSupplier If true, the robot heading will be set to move towards our alliance wall
* @param leftSupplier If true, the robot heading will be set to move towards the left
* @param rightSupplier If true, the robot heading will be set to move towards the right
*/
public DriveFieldOrientedHeadingSnapping(
Supplier<Double> xSupplier,
Supplier<Double> ySupplier,
Supplier<Double> zSupplier,
Supplier<Boolean> upSupplier,
Supplier<Boolean> downSupplier,
Supplier<Boolean> leftSupplier,
Supplier<Boolean> rightSupplier) {
this.controller = RobotContainer.driverController.getHID();
this.drive = RobotContainer.drive;
this.xSupplier = xSupplier;
this.ySupplier = ySupplier;
this.zSupplier = zSupplier;
this.upSupplier = upSupplier;
this.downSupplier = downSupplier;
this.leftSupplier = leftSupplier;
this.rightSupplier = rightSupplier;
addRequirements(RobotContainer.drive);
}

Expand All @@ -46,33 +75,25 @@ public void initialize() {
public void execute() {
double headingX = 0;
double headingY = 0;
if (controller.getPOV() == 0) {
if (upSupplier.get()) {
headingY = -1;
}
if (controller.getPOV() == 90) {
if (rightSupplier.get()) {
headingX = 1;
}
if (controller.getPOV() == 180) {
if (downSupplier.get()) {
headingY = 1;
}
if (controller.getPOV() == 270) {
if (leftSupplier.get()) {
headingX = -1;
}

ChassisSpeeds desiredSpeeds =
drive.getTargetSpeeds(
RobotContainer.getScaledControllerLeftXAxis(),
RobotContainer.getScaledControllerLeftYAxis(),
headingX,
headingY);
drive.getTargetSpeeds(xSupplier.get(), ySupplier.get(), headingX, headingY);

Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
if (headingX == 0
&& headingY == 0
&& Math.abs(RobotContainer.getScaledControllerRightXAxis()) > 0) {
drive.driveFieldOriented(
translation,
(RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED));
if (headingX == 0 && headingY == 0 && Math.abs(zSupplier.get()) > 0) {
drive.driveFieldOriented(translation, zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED);
} else {
drive.driveFieldOriented(translation, desiredSpeeds.omegaRadiansPerSecond);
}
Expand Down
22 changes: 17 additions & 5 deletions src/main/java/frc/robot/commands/drive/DriveRobotOriented.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,14 +16,26 @@
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.subsystems.Drive;
import java.util.function.Supplier;

/** Drives the robot in robot-oriented mode. Default command for {@link Drive} subsystem. */
public class DriveRobotOriented extends Command {
private final Drive drive;
private final Supplier<Double> xSupplier, ySupplier, zSupplier;

/** Creates a new DriveRobotOriented. */
public DriveRobotOriented() {
/**
* Drives the robot robot-oriented
*
* @param xSupplier The joystick value for the Y axis. [-1, 1] left positive.
* @param ySupplier The joystick value for the Y axis. [-1, 1] back positive.
* @param zSupplier The joystick value for the Z axis. [-1, 1] counterclockwise positive.
*/
public DriveRobotOriented(
Supplier<Double> xSupplier, Supplier<Double> ySupplier, Supplier<Double> zSupplier) {
this.drive = RobotContainer.drive;
this.xSupplier = xSupplier;
this.ySupplier = ySupplier;
this.zSupplier = zSupplier;
addRequirements(drive);
}

Expand All @@ -34,9 +46,9 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED;
double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED;
double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED;
double x = xSupplier.get() * Constants.Drive.MAX_SPEED;
double y = ySupplier.get() * Constants.Drive.MAX_SPEED;
double z = zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED;
Translation2d translation = new Translation2d(x, y);

drive.driveRobotOriented(translation, z);
Expand Down

0 comments on commit f08d0ac

Please sign in to comment.