-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* PP wrapper * Working * Apply Prettier format * Remove un-used debug prints * Apply Prettier format * Add back to off triggers --------- Co-authored-by: github-actions <[email protected]>
- Loading branch information
1 parent
b9fe126
commit 2057a1b
Showing
3 changed files
with
106 additions
and
72 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
71 changes: 71 additions & 0 deletions
71
src/main/java/frc/robot/subsystems/drive/PPDriveWrapper.java
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,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 | ||
| ); | ||
| } | ||
| } |