From 2057a1bc07ce2d0b93d0fd0f35d3321b56f64e7d Mon Sep 17 00:00:00 2001 From: Otto <105565584+GreenTomato5@users.noreply.github.com> Date: Sun, 4 Aug 2024 08:19:18 -0800 Subject: [PATCH] Re subsystem abstract drive (#18) * PP wrapper * Working * Apply Prettier format * Remove un-used debug prints * Apply Prettier format * Add back to off triggers --------- Co-authored-by: github-actions --- src/main/java/frc/robot/Robot.java | 6 +- .../frc/robot/subsystems/drive/Drive.java | 101 ++++++------------ .../subsystems/drive/PPDriveWrapper.java | 71 ++++++++++++ 3 files changed, 106 insertions(+), 72 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 34958f8..d3c6f8e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -86,6 +86,8 @@ public void robotInit() { case SIM: // Running a physics simulator, log to NT Logger.addDataReceiver(new NT4Publisher()); + // Logger.addDataReceiver(new WPILOGWriter("C:\\Dev\\Robotics + // Related\\Controls\\2024-Rewrite\\logs", defaultPeriodSecs)); break; case REPLAY: // Replaying a log, set up replay source @@ -103,8 +105,8 @@ public void robotInit() { // Start AdvantageKit logger Logger.start(); - - autoChooser = AutoBuilder.buildAutoChooser(); + // TODO: Make default auto a "cross line" auto + autoChooser = AutoBuilder.buildAutoChooser("Example Auto"); SmartDashboard.putData("Auto Chooser", autoChooser); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8fc9490..12a648d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,9 +13,6 @@ package frc.robot.subsystems.drive; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -29,23 +26,21 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.subsystems.Subsystem; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Drive extends SubsystemBase { +public class Drive extends Subsystem { static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR - - private DriveStates state; - private String subsystemName; + private PPDriveWrapper autoConfig; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -70,8 +65,8 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO ) { - subsystemName = "Drive"; - state = DriveStates.REGULAR_DRIVE; + super("Drive", DriveStates.REGULAR_DRIVE); + autoConfig = new PPDriveWrapper(this); this.gyroIO = gyroIO; modules[0] = new Module(flModuleIO, 0); @@ -83,26 +78,36 @@ public Drive( PhoenixOdometryThread.getInstance().start(); SparkMaxOdometryThread.getInstance().start(); - pathPlannerInit(); - } - - public void runState() { - drive( - this, - () -> Constants.controller.getLeftY(), - () -> Constants.controller.getLeftX(), - () -> -Constants.controller.getRightX(), - getState().getRotationModifier(), - getState().getTranslationModifier() + // Triggers + addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SLOW_MODE, () -> + Constants.controller.getRightBumper() + ); + addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SPEED_MAXXING, () -> + Constants.controller.getLeftBumper() ); - } - public DriveStates getState() { - return state; + // Back to Off + addTrigger(DriveStates.SPEED_MAXXING, DriveStates.SLOW_MODE, () -> + Constants.controller.getLeftBumperReleased() + ); + addTrigger(DriveStates.SLOW_MODE, DriveStates.REGULAR_DRIVE, () -> + Constants.controller.getRightBumperReleased() + ); } - public void setState(DriveStates state) { - this.state = state; + @Override + public void runState() { + // Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not good) + if (DriverStation.isTeleop()) { + drive( + this, + () -> Constants.controller.getLeftY(), + () -> Constants.controller.getLeftX(), + () -> -Constants.controller.getRightX(), + getState().getRotationModifier(), + getState().getTranslationModifier() + ); + } } public void drive( @@ -212,18 +217,6 @@ public void periodic() { // Apply update poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } - - if (Constants.controller.getRightBumper() && getState() == DriveStates.REGULAR_DRIVE) { - setState(DriveStates.SLOW_MODE); - } else if ( - Constants.controller.getLeftBumper() && getState() == DriveStates.REGULAR_DRIVE - ) { - setState(DriveStates.SPEED_MAXXING); - } else if ( - !Constants.controller.getRightBumper() && !Constants.controller.getLeftBumper() - ) { - setState(DriveStates.REGULAR_DRIVE); - } } /** @@ -363,36 +356,4 @@ public static Translation2d[] getModuleTranslations() { ), }; } - - public void pathPlannerInit() { - AutoBuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( - // HolonomicPathFollowerConfig, this should likely live in - // your Constants class - Constants.Drive.TRANSLATION_PID, // Translation PID constants - Constants.Drive.ROTATION_PID, // Rotation PID constants - Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s - Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to - // furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options - // here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java new file mode 100644 index 0000000..7609aac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.drive; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class PPDriveWrapper extends SubsystemBase { + + Drive drive; + + PPDriveWrapper(Drive drive) { + this.drive = drive; + pathPlannerInit(); + } + + private Pose2d getPose() { + return drive.getPose(); + } + + private void setPose(Pose2d pose) { + drive.setPose(pose); + } + + private ChassisSpeeds getChassisSpeed() { + return drive.getChassisSpeed(); + } + + private void runVelocity(ChassisSpeeds chassisSpeeds) { + drive.runVelocity(chassisSpeeds); + } + + public void periodic() {} + + public void pathPlannerInit() { + AutoBuilder.configureHolonomic( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( + // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + Constants.Drive.TRANSLATION_PID, // Translation PID constants + Constants.Drive.ROTATION_PID, // Rotation PID constants + Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s + Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + // Replans path if vision or odo detects errors (S tier) + new ReplanningConfig(true, true) // Default path replanning config. See the API for the options + // here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } +}