Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +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

16
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
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. */
@@ -48,6 +48,7 @@ public class RamseteController {
private Pose2d m_poseError = new Pose2d();
private Pose2d m_poseTolerance = new Pose2d();
private boolean m_enabled = true;
/**
* Construct a Ramsete unicycle controller.
@@ -111,6 +112,10 @@ public class RamseteController {
Pose2d poseRef,
double linearVelocityRefMeters,
double angularVelocityRefRadiansPerSecond) {
if (!m_enabled) {
return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
}
m_poseError = poseRef.relativeTo(currentPose);
// Aliases for equation readability
@@ -143,6 +148,15 @@ public class RamseteController {
desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
}
/**
* Enables and disables the controller for troubleshooting purposes.
*
* @param enabled If the controller is enabled or not.
*/
public void setEnabled(boolean enabled) {
m_enabled = enabled;
}
/**
* Returns sin(x) / x.
*