From 3f5779eda7e88ff987db7a715ca7d8b168a97025 Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla <49494444+BrownGenius@users.noreply.github.com> Date: Sat, 27 Jan 2024 14:21:55 -0500 Subject: [PATCH 1/2] Fix field oriented drive (#19) * "back/select" button resets odometry * Increased joystick deadband to prevent drift --- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- .../frc/robot/subsystems/drive/DriveSwerveYAGSL.java | 10 ++++++++-- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6080f156..853fa2b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,15 +62,17 @@ private void configureBindings() { drive.setDefaultCommand( new DriveCommand( drive, - () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01), - () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01), - () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01))); + () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.05), + () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.05), + () -> MathUtil.applyDeadband(-controller.getRightX(), 0.05))); // TODO: Move deadband to constants file controller .start() .onTrue( new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive()))); + + controller.back().onTrue(new InstantCommand(() -> drive.resetOdometry())); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java index 5f902615..dc1fe07b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java @@ -1,19 +1,21 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; import java.io.File; +import org.littletonrobotics.junction.AutoLogOutput; import swervelib.SwerveDrive; import swervelib.parser.SwerveParser; public class DriveSwerveYAGSL extends DriveBase { private final double maximumSpeed = - Units.feetToMeters(2); // * TODO: Calculate actual max speed */ + Units.feetToMeters(4.5); // * TODO: Calculate actual max speed */ private final File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "swervePracticeBot"); private SwerveDrive swerveDrive; - private boolean fieldOrientedDrive = false; + @AutoLogOutput private boolean fieldOrientedDrive = false; public DriveSwerveYAGSL() { try { @@ -49,4 +51,8 @@ public void setFieldOrientedDrive(boolean enable) { public boolean isFieldOrientedDrive() { return fieldOrientedDrive; } + + public void resetOdometry() { + swerveDrive.resetOdometry(new Pose2d()); + } } From 57d6b1fdbe87f796e72007d1ac43d3819feb2336 Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Sat, 27 Jan 2024 15:42:14 -0500 Subject: [PATCH 2/2] Simple robot multi config --- src/main/java/frc/robot/RobotContainer.java | 40 ++++++++++++++++++- .../frc/robot/subsystems/drive/Drive.java | 10 +++++ 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 853fa2b1..048ed984 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import frc.robot.commands.IntakeBaseCommand; import frc.robot.commands.ShooterEnable; import frc.robot.commands.drive.DriveCommand; +import frc.robot.subsystems.drive.DriveBase; import frc.robot.subsystems.drive.DriveSwerveYAGSL; import frc.robot.subsystems.intake.IntakeBase; import frc.robot.subsystems.intake.IntakeIOSim; @@ -24,13 +25,39 @@ public class RobotContainer { public final CommandXboxController controller; public final ShooterSubsystem shooter; public final IntakeBase intake; - public final DriveSwerveYAGSL drive; + public final DriveBase drive; + + public enum RobotModel { + PHOENIX, // Practice Swerve Bot + SHERMAN, // Practice Tank Bot + } + + public enum DriveType { + NONE, + SWERVE, + TANK + } public RobotContainer() { + RobotModel model = RobotModel.PHOENIX; + controller = new CommandXboxController(0); boolean hasIntake = false; boolean hasShooter = false; + DriveType driveType = DriveType.NONE; + + switch (model) { + case PHOENIX: + driveType = DriveType.SWERVE; + break; + case SHERMAN: + driveType = DriveType.TANK; + hasIntake = true; + hasShooter = true; + break; + default: + } if (hasShooter) { shooter = new ShooterSubsystem(new ShooterIOSparkMax()); @@ -44,7 +71,16 @@ public RobotContainer() { intake = new IntakeBase(new IntakeIOSim()); } - drive = new DriveSwerveYAGSL(); + switch (driveType) { + case SWERVE: + drive = new DriveSwerveYAGSL(); + break; + case TANK: + drive = null; + break; + default: + drive = null; + } configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a309baaa..3c90dca9 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -23,4 +23,14 @@ public interface Drive { * @return speed in radian/sec */ public double getMaxAngularSpeed(); + + public default void setFieldOrientedDrive(boolean enable) {} + ; + + public default boolean isFieldOrientedDrive() { + return false; + } + + public default void resetOdometry() {} + ; }