From ff5d3e5b36662feb3bb88b3df2b9525581c5b896 Mon Sep 17 00:00:00 2001
From: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
Date: Mon, 27 Jan 2020 21:53:00 -0800
Subject: [PATCH] RamseteController: Add setEnabled method (#2313)

---
 .../native/cpp/controller/RamseteController.cpp  |  8 +++++++-
 .../include/frc/controller/RamseteController.h   | 10 +++++++++-
 .../wpilibj/controller/RamseteController.java    | 16 +++++++++++++++-
 3 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
index c55c2e8c0f..0302d187d2 100644
--- a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
@@ -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; }
diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
index 6ec6edc292..e099b5be12 100644
--- a/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -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
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
index 3ff9ac5b95..a682923582 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
@@ -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.
    *