Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rename HolonomicDriveController.calculate params (#4683)

Browse Source
This commit is contained in:
Starlight220
2022-11-24 09:13:50 +02:00
committed by GitHub
parent 2ee3d86de4
commit d5200db6cd
3 changed files with 53 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -75,14 +75,17 @@ public class HolonomicDriveController {
/**
* Returns the next output of the holonomic drive controller.
*
* @param currentPose The current pose.
* @param poseRef The desired pose.
* @param linearVelocityRefMeters The linear velocity reference.
* @param angleRef The angular reference.
* @param currentPose The current pose, as measured by odometry or pose estimator.
* @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
* @param desiredLinearVelocityMetersPerSecond The desired linear velocity.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
public ChassisSpeeds calculate(
Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
Pose2d currentPose,
Pose2d trajectoryPose,
double desiredLinearVelocityMetersPerSecond,
Rotation2d desiredHeading) {
// If this is the first run, then we need to reset the theta controller to the current pose's
// heading.
if (m_firstRun) {
@@ -91,21 +94,22 @@ public class HolonomicDriveController {
}
// Calculate feedforward velocities (field-relative).
double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos();
double yFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getSin();
double thetaFF =
m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
m_thetaController.calculate(
currentPose.getRotation().getRadians(), desiredHeading.getRadians());
m_poseError = poseRef.relativeTo(currentPose);
m_rotationError = angleRef.minus(currentPose.getRotation());
m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
if (!m_enabled) {
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
}
// Calculate feedback velocities (based on position error).
double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
return ChassisSpeeds.fromFieldRelativeSpeeds(
@@ -115,15 +119,15 @@ public class HolonomicDriveController {
/**
* Returns the next output of the holonomic drive controller.
*
* @param currentPose The current pose.
* @param desiredState The desired trajectory state.
* @param angleRef The desired end-angle.
* @param currentPose The current pose, as measured by odometry or pose estimator.
* @param desiredState The desired trajectory pose, as sampled for the current timestep.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
public ChassisSpeeds calculate(
Pose2d currentPose, Trajectory.State desiredState, Rotation2d angleRef) {
Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
return calculate(
currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, angleRef);
currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, desiredHeading);
}
/**