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

[wpimath] Remove RamseteController and RamseteCommand (#7522)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-07 23:38:35 -08:00
committed by GitHub
parent ae44295024
commit 220f4e1ba4
14 changed files with 8 additions and 996 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
View File

@@ -1,133 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc2/command/RamseteCommand.h"
#include <utility>
#include <units/velocity.h>
#include <units/voltage.h>
#include <wpi/sendable/SendableBuilder.h>
using namespace frc2;
using kv_unit = units::compound_unit<units::volts,
units::inverse<units::meters_per_second>>;
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
frc::PIDController leftController, frc::PIDController rightController,
std::function<void(units::volt_t, units::volt_t)> output,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_controller(controller),
m_feedforward(feedforward),
m_kinematics(std::move(kinematics)),
m_speeds(std::move(wheelSpeeds)),
m_leftController(std::make_unique<frc::PIDController>(leftController)),
m_rightController(std::make_unique<frc::PIDController>(rightController)),
m_outputVolts(std::move(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,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_controller(controller),
m_feedforward(0_V, units::unit_t<kv_unit>{0}),
m_kinematics(std::move(kinematics)),
m_outputVel(std::move(output)),
m_usePID(false) {
AddRequirements(requirements);
}
void RamseteCommand::Initialize() {
m_prevTime = -1_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.Restart();
if (m_usePID) {
m_leftController->Reset();
m_rightController->Reset();
}
}
void RamseteCommand::Execute() {
auto curTime = m_timer.Get();
if (m_prevTime < 0_s) {
if (m_usePID) {
m_outputVolts(0_V, 0_V);
} else {
m_outputVel(0_mps, 0_mps);
}
m_prevTime = curTime;
return;
}
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
if (m_usePID) {
auto leftFeedforward =
m_feedforward.Calculate(m_prevSpeeds.left, targetWheelSpeeds.left);
auto rightFeedforward =
m_feedforward.Calculate(m_prevSpeeds.right, targetWheelSpeeds.right);
auto leftOutput =
units::volt_t{m_leftController->Calculate(
m_speeds().left.value(), targetWheelSpeeds.left.value())} +
leftFeedforward;
auto rightOutput =
units::volt_t{m_rightController->Calculate(
m_speeds().right.value(), targetWheelSpeeds.right.value())} +
rightFeedforward;
m_outputVolts(leftOutput, rightOutput);
} else {
m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
}
m_prevSpeeds = targetWheelSpeeds;
m_prevTime = curTime;
}
void RamseteCommand::End(bool interrupted) {
m_timer.Stop();
if (interrupted) {
if (m_usePID) {
m_outputVolts(0_V, 0_V);
} else {
m_outputVel(0_mps, 0_mps);
}
}
}
bool RamseteCommand::IsFinished() {
return m_timer.HasElapsed(m_trajectory.TotalTime());
}
void RamseteCommand::InitSendable(wpi::SendableBuilder& builder) {
Command::InitSendable(builder);
builder.AddDoubleProperty(
"leftVelocity", [this] { return m_prevSpeeds.left.value(); }, nullptr);
builder.AddDoubleProperty(
"rightVelocity", [this] { return m_prevSpeeds.right.value(); }, nullptr);
}

138
wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
View File

@@ -1,138 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <functional>
#include <memory>
#include <frc/Timer.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/RamseteController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/trajectory/Trajectory.h>
#include <units/length.h>
#include <units/voltage.h>
#include "frc2/command/Command.h"
#include "frc2/command/CommandHelper.h"
#include "frc2/command/Requirements.h"
namespace frc2 {
/**
* A command that uses a RAMSETE controller to follow a trajectory
* with a differential drive.
*
* <p>The command handles trajectory-following, PID calculations, and
* feedforwards internally. This is intended to be a more-or-less "complete
* solution" that can be used by teams without a great deal of controls
* expertise.
*
* <p>Advanced teams seeking more flexibility (for example, those who wish to
* use the onboard PID functionality of a "smart" motor controller) may use the
* secondary constructor that omits the PID and feedforward functionality,
* returning only the raw wheel speeds from the RAMSETE controller.
*
* This class is provided by the NewCommands VendorDep
*
* @see RamseteController
* @see Trajectory
*/
class RamseteCommand : public CommandHelper<Command, RamseteCommand> {
public:
/**
* Constructs a new RamseteCommand that, when executed, will follow the
* provided trajectory. PID control and feedforward are handled internally,
* and outputs are scaled -12 to 12 representing units of volts.
*
* <p>Note: The controller will *not* set the outputVolts to zero upon
* completion of the path - this is left to the user, since it is not
* appropriate for paths with nonstationary endstates.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param controller The RAMSETE controller used to follow the
* trajectory.
* @param feedforward A component for calculating the feedforward for the
* drive.
* @param kinematics The kinematics for the robot drivetrain.
* @param wheelSpeeds A function that supplies the speeds of the left
* and right sides of the robot drive.
* @param leftController The PIDController for the left side of the robot
* drive.
* @param rightController The PIDController for the right side of the robot
* drive.
* @param output A function that consumes the computed left and right
* outputs (in volts) for the robot drive.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
[[deprecated("Use LTVUnicycleController instead.")]]
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
frc::PIDController leftController,
frc::PIDController rightController,
std::function<void(units::volt_t, units::volt_t)> output,
Requirements requirements = {});
/**
* Constructs a new RamseteCommand that, when executed, will follow the
* provided trajectory. Performs no PID control and calculates no
* feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
* and will need to be converted into a usable form by the user.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of
* the odometry classes to provide this.
* @param controller The RAMSETE controller used to follow the
* trajectory.
* @param kinematics The kinematics for the robot drivetrain.
* @param output A function that consumes the computed left and right
* wheel speeds.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
[[deprecated("Use LTVUnicycleController instead.")]]
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,
Requirements requirements = {});
void Initialize() override;
void Execute() override;
void End(bool interrupted) override;
bool IsFinished() override;
void InitSendable(wpi::SendableBuilder& builder) override;
private:
frc::Trajectory m_trajectory;
std::function<frc::Pose2d()> m_pose;
frc::RamseteController m_controller;
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
frc::DifferentialDriveKinematics m_kinematics;
std::function<frc::DifferentialDriveWheelSpeeds()> m_speeds;
std::unique_ptr<frc::PIDController> m_leftController;
std::unique_ptr<frc::PIDController> m_rightController;
std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
m_outputVel;
frc::Timer m_timer;
units::second_t m_prevTime;
frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
bool m_usePID;
};
} // namespace frc2