CS20 should be done, CS21 is close

This commit is contained in:
2024-07-16 19:03:42 -04:00
parent 6d61a99e4b
commit afe6487706
31 changed files with 1919 additions and 5 deletions

View File

@@ -4,7 +4,11 @@
package frc.robot;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj2.command.Command;
@@ -23,7 +27,7 @@ public class RobotContainer {
private Drivetrain drivetrain;
// The driver's controller
CommandXboxController driver;
private CommandXboxController driver;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -51,7 +55,16 @@ public class RobotContainer {
drivetrain.drive(
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
driver::getRightX,
() -> {
Optional<Alliance> alliance = DriverStation.getAlliance();
if (alliance.isEmpty() || alliance.get() == Alliance.Blue) {
return false;
} else {
return true;
}
},
OIConstants.kDriveDeadband,
true,
true

View File

@@ -4,6 +4,7 @@
package frc.robot.subsystems;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.MathUtil;
@@ -133,13 +134,13 @@ public class Drivetrain extends SubsystemBase {
pose);
}
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot, BooleanSupplier redInvert,
double deadband, boolean fieldRelative, boolean rateLimit) {
return run(() -> {
drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband) * (fieldRelative && redInvert.getAsBoolean() ? -1 : 1),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband) * (fieldRelative && redInvert.getAsBoolean() ? -1 : 1),
-MathUtil.applyDeadband(rot.getAsDouble(), deadband),
fieldRelative,
rateLimit