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

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) {