FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

PathPlanner Helpers

This commit is contained in:
Tyler Wilcox 2026-03-20 17:01:57 -04:00
parent 386a892d62
commit 425745f11c
1 changed files with 32 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
View File

@ -7,6 +7,7 @@ import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Rotation3d;
@ -95,7 +96,7 @@ public class SwerveDriveSystem implements Subsystem{
double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds()); double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds());
if(mLastSimTime != 0){ if(mLastSimTime != 0){
Pose3d oldPose = getRobotPose(); Pose3d oldPose = getRobotPose3d();
poseEstimator.resetPose(new Pose3d(oldPose.getX() + (chassisSpeeds.vxMetersPerSecond * (deltaTimeSecond)), oldPose.getY() + chassisSpeeds.vyMetersPerSecond * (deltaTimeSecond), 0, oldPose.getRotation().plus(new Rotation3d(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * (deltaTimeSecond)))))); poseEstimator.resetPose(new Pose3d(oldPose.getX() + (chassisSpeeds.vxMetersPerSecond * (deltaTimeSecond)), oldPose.getY() + chassisSpeeds.vyMetersPerSecond * (deltaTimeSecond), 0, oldPose.getRotation().plus(new Rotation3d(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * (deltaTimeSecond))))));
} }
@ -159,7 +160,7 @@ public class SwerveDriveSystem implements Subsystem{
* @param chassisSpeeds * @param chassisSpeeds
* @param centerOfRotation * @param centerOfRotation
*/ */
private void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){ public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
this.chassisSpeeds = chassisSpeeds; this.chassisSpeeds = chassisSpeeds;
this.centerOfRotation = centerOfRotation; this.centerOfRotation = centerOfRotation;
} }
@ -194,14 +195,42 @@ public class SwerveDriveSystem implements Subsystem{
), new Translation2d()); ), new Translation2d());
} }
public ChassisSpeeds getRobotRelativeSpeeds(){
return this.chassisSpeeds;
}
/** /**
* Get the current field location of the robot * Get the current field location of the robot
* @return The current pose as a Pose3d * @return The current pose as a Pose3d
*/ */
public Pose3d getRobotPose(){ public Pose3d getRobotPose3d(){
return poseEstimator.getEstimatedPosition(); return poseEstimator.getEstimatedPosition();
} }
/**
* Get the current field location of the robot
* @return The current pose as a Pose2d
*/
public Pose2d getRobotPose2d(){
return poseEstimator.getEstimatedPosition().toPose2d();
}
/**
* Set pose estimation to a specific pose
* @param pose The new pose as a Pose3d
*/
public void setPose3d(Pose3d pose){
poseEstimator.resetPose(pose);
}
/**
* Set pose estimation to a specific pose
* @param pose The new pose as a Pose2d
*/
public void setPose2d(Pose2d pose){
this.setPose3d(new Pose3d(pose));
}
private SwerveModulePosition[] getModulePositions(){ private SwerveModulePosition[] getModulePositions(){
return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new); return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new);
} }