Split the two command implementations into separate libraries (#2012)

This will allow us at the user code side to determine to include old commands, new commands or both.
2026-06-20 00:51:42 +00:00 · 2019-11-01 21:58:54 -07:00
parent 2ad15cae19
commit 509819d83f
271 changed files with 470 additions and 91 deletions
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RamseteCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller, volt_t ks,
+    units::unit_t<voltsecondspermeter> kv,
+    units::unit_t<voltsecondssquaredpermeter> ka,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<units::meters_per_second_t()> leftSpeed,
+    std::function<units::meters_per_second_t()> rightSpeed,
+    frc2::PIDController leftController, frc2::PIDController rightController,
+    std::function<void(volt_t, volt_t)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(ks),
+      m_kv(kv),
+      m_ka(ka),
+      m_kinematics(kinematics),
+      m_leftSpeed(leftSpeed),
+      m_rightSpeed(rightSpeed),
+      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(0),
+      m_kv(0),
+      m_ka(0),
+      m_kinematics(kinematics),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+}
+
+void RamseteCommand::Initialize() {
+  m_prevTime = 0_s;
+  auto initialState = m_trajectory.Sample(0_s);
+  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+      frc::ChassisSpeeds{initialState.velocity, 0_mps,
+                         initialState.velocity * initialState.curvature});
+  m_timer.Reset();
+  m_timer.Start();
+  if (m_usePID) {
+    m_leftController->Reset();
+    m_rightController->Reset();
+  }
+}
+
+void RamseteCommand::Execute() {
+  auto curTime = m_timer.Get();
+  auto dt = curTime - m_prevTime;
+
+  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
+      m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
+
+  if (m_usePID) {
+    auto leftFeedforward =
+        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
+        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+
+    auto rightFeedforward =
+        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
+        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+
+    auto leftOutput =
+        volt_t(m_leftController->Calculate(
+            m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
+        leftFeedforward;
+
+    auto rightOutput = volt_t(m_rightController->Calculate(
+                           m_rightSpeed().to<double>(),
+                           targetWheelSpeeds.right.to<double>())) +
+                       rightFeedforward;
+
+    m_outputVolts(leftOutput, rightOutput);
+  } else {
+    m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
+  }
+
+  m_prevTime = curTime;
+  m_prevSpeeds = targetWheelSpeeds;
+}
+
+void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool RamseteCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}