Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

RamseteController: Add setEnabled method (#2313)

Browse Source
This commit is contained in:
Declan Freeman-Gleason
2020-01-27 21:53:00 -08:00
committed by Peter Johnson
parent 6cc68ab503
commit ff5d3e5b36
3 changed files with 31 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpilibc/src/main/native/cpp/controller/RamseteController.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -45,6 +45,10 @@ ChassisSpeeds RamseteController::Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef,
units::radians_per_second_t angularVelocityRef) {
if (!m_enabled) {
return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
}
m_poseError = poseRef.RelativeTo(currentPose);
// Aliases for equation readability
@@ -68,3 +72,5 @@ ChassisSpeeds RamseteController::Calculate(
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredState.velocity * desiredState.curvature);
}
void RamseteController::SetEnabled(bool enabled) { m_enabled = enabled; }

10
wpilibc/src/main/native/include/frc/controller/RamseteController.h
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -102,12 +102,20 @@ class RamseteController {
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState);
/**
* Enables and disables the controller for troubleshooting purposes.
*
* @param enabled If the controller is enabled or not.
*/
void SetEnabled(bool enabled);
private:
double m_b;
double m_zeta;
Pose2d m_poseError;
Pose2d m_poseTolerance;
bool m_enabled = true;
};
} // namespace frc