Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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);
}
/**

27
wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
View File

@@ -34,8 +34,9 @@ void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
}
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
const Pose2d& currentPose, const Pose2d& trajectoryPose,
units::meters_per_second_t desiredLinearVelocity,
const 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) {
@@ -44,13 +45,13 @@ ChassisSpeeds HolonomicDriveController::Calculate(
}
// Calculate feedforward velocities (field-relative)
auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
currentPose.Rotation().Radians(), angleRef.Radians())};
currentPose.Rotation().Radians(), desiredHeading.Radians())};
m_poseError = poseRef.RelativeTo(currentPose);
m_rotationError = angleRef - currentPose.Rotation();
m_poseError = trajectoryPose.RelativeTo(currentPose);
m_rotationError = desiredHeading - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
@@ -58,10 +59,10 @@ ChassisSpeeds HolonomicDriveController::Calculate(
}
// Calculate feedback velocities (based on position error).
auto xFeedback = units::meters_per_second_t{
m_xController.Calculate(currentPose.X().value(), poseRef.X().value())};
auto yFeedback = units::meters_per_second_t{
m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value())};
auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
currentPose.X().value(), trajectoryPose.X().value())};
auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
currentPose.Y().value(), trajectoryPose.Y().value())};
// Return next output.
return ChassisSpeeds::FromFieldRelativeSpeeds(
@@ -70,9 +71,9 @@ ChassisSpeeds HolonomicDriveController::Calculate(
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Trajectory::State& desiredState,
const Rotation2d& angleRef) {
const Rotation2d& desiredHeading) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
angleRef);
desiredHeading);
}
void HolonomicDriveController::SetEnabled(bool enabled) {

36
wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -61,32 +61,32 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
/**
* Returns the next output of the holonomic drive controller.
*
* The reference pose, linear velocity, and angular velocity should come from
* a drivetrain trajectory.
*
* @param currentPose The current pose.
* @param poseRef The desired pose.
* @param linearVelocityRef The desired linear velocity.
* @param angleRef The desired ending angle.
* @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 desiredLinearVelocity The desired linear velocity.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef,
const Rotation2d& angleRef);
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Pose2d& trajectoryPose,
units::meters_per_second_t desiredLinearVelocity,
const Rotation2d& desiredHeading);
/**
* Returns the next output of the holonomic drive controller.
*
* The reference pose, linear velocity, and angular velocity should come from
* a drivetrain trajectory.
*
* @param currentPose The current pose.
* @param desiredState The desired pose, linear velocity, and angular velocity
* from a trajectory.
* @param angleRef The desired ending 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.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState,
const Rotation2d& angleRef);
const Rotation2d& desiredHeading);
/**
* Enables and disables the controller for troubleshooting purposes. When