Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

227
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -1,227 +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.
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.BiConsumer;
import java.util.function.Supplier;
/**
* A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
* {@link 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.
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class RamseteCommand extends Command {
private final Timer m_timer = new Timer();
private final boolean m_usePID;
private final Trajectory m_trajectory;
private final Supplier<Pose2d> m_pose;
private final RamseteController m_follower;
private final SimpleMotorFeedforward m_feedforward;
private final DifferentialDriveKinematics m_kinematics;
private final Supplier<DifferentialDriveWheelSpeeds> m_speeds;
private final PIDController m_leftController;
private final PIDController m_rightController;
private final BiConsumer<Double, Double> m_output;
private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds();
private double m_prevLeftSpeedSetpoint; // m/s
private double m_prevRightSpeedSetpoint; // m/s
private double m_prevTime;
/**
* 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 The feedforward to use 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 outputVolts 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(since = "2025", forRemoval = true)
@SuppressWarnings("this-escape")
public RamseteCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
RamseteController controller,
SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
PIDController leftController,
PIDController rightController,
BiConsumer<Double, Double> outputVolts,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
m_feedforward = feedforward;
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
m_speeds = requireNonNullParam(wheelSpeeds, "wheelSpeeds", "RamseteCommand");
m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
m_usePID = true;
addRequirements(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 follower The RAMSETE follower used to follow the trajectory.
* @param kinematics The kinematics for the robot drivetrain.
* @param outputMetersPerSecond A function that consumes the computed left and right wheel speeds.
* @param requirements The subsystems to require.
* @deprecated Use LTVUnicycleController instead.
*/
@Deprecated(since = "2025", forRemoval = true)
@SuppressWarnings("this-escape")
public RamseteCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
RamseteController follower,
DifferentialDriveKinematics kinematics,
BiConsumer<Double, Double> outputMetersPerSecond,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
m_output =
requireNonNullParam(outputMetersPerSecond, "outputMetersPerSecond", "RamseteCommand");
m_feedforward = null;
m_speeds = null;
m_leftController = null;
m_rightController = null;
m_usePID = false;
addRequirements(requirements);
}
@Override
public void initialize() {
m_prevTime = -1;
var initialState = m_trajectory.sample(0);
m_prevSpeeds =
m_kinematics.toWheelSpeeds(
new ChassisSpeeds(
initialState.velocityMetersPerSecond,
0,
initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond));
m_prevLeftSpeedSetpoint = m_prevSpeeds.leftMetersPerSecond;
m_prevRightSpeedSetpoint = m_prevSpeeds.rightMetersPerSecond;
m_timer.restart();
if (m_usePID) {
m_leftController.reset();
m_rightController.reset();
}
}
@Override
public void execute() {
double curTime = m_timer.get();
if (m_prevTime < 0) {
m_output.accept(0.0, 0.0);
m_prevTime = curTime;
return;
}
var targetWheelSpeeds =
m_kinematics.toWheelSpeeds(
m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
double leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
double rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
double leftOutput;
double rightOutput;
if (m_usePID) {
double leftFeedforward = m_feedforward.calculate(m_prevLeftSpeedSetpoint, leftSpeedSetpoint);
double rightFeedforward =
m_feedforward.calculate(m_prevRightSpeedSetpoint, rightSpeedSetpoint);
leftOutput =
leftFeedforward
+ m_leftController.calculate(m_speeds.get().leftMetersPerSecond, leftSpeedSetpoint);
rightOutput =
rightFeedforward
+ m_rightController.calculate(
m_speeds.get().rightMetersPerSecond, rightSpeedSetpoint);
} else {
leftOutput = leftSpeedSetpoint;
rightOutput = rightSpeedSetpoint;
}
m_output.accept(leftOutput, rightOutput);
m_prevSpeeds = targetWheelSpeeds;
m_prevTime = curTime;
}
@Override
public void end(boolean interrupted) {
m_timer.stop();
if (interrupted) {
m_output.accept(0.0, 0.0);
}
}
@Override
public boolean isFinished() {
return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addDoubleProperty("leftVelocity", () -> m_prevSpeeds.leftMetersPerSecond, null);
builder.addDoubleProperty("rightVelocity", () -> m_prevSpeeds.rightMetersPerSecond, null);
}
}

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