mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-06 03:31:43 +00:00
Add RamseteCommand (#1951)
This commit is contained in:
Oblarg
113
wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
Normal file
113
wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
Normal file
@@ -0,0 +1,113 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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) {
|
||||
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) {
|
||||
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();
|
||||
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_leftController.get() != nullptr) {
|
||||
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());
|
||||
}
|
||||
147
wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
Normal file
147
wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
Normal file
@@ -0,0 +1,147 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 <functional>
|
||||
#include <memory>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "CommandBase.h"
|
||||
#include "CommandHelper.h"
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc/controller/RamseteController.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "frc2/Timer.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
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.
|
||||
*
|
||||
* @see RamseteController
|
||||
* @see Trajectory
|
||||
*/
|
||||
class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
|
||||
using voltsecondspermeter =
|
||||
units::compound_unit<units::volt, units::second,
|
||||
units::inverse<units::meter>>;
|
||||
using voltsecondssquaredpermeter =
|
||||
units::compound_unit<units::volt, units::squared<units::second>,
|
||||
units::inverse<units::meter>>;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Constructs a new RamseteCommand that, when executed, will follow the
|
||||
* provided trajectory. PID control and feedforward are handled internally,
|
||||
* and outputs are scaled -1 to 1 for easy consumption by speed controllers.
|
||||
*
|
||||
* <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 ks Constant feedforward term for the robot drive.
|
||||
* @param kv Velocity-proportional feedforward term for the robot
|
||||
* drive.
|
||||
* @param ka Acceleration-proportional feedforward term for the
|
||||
* robot drive.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param leftSpeed A function that supplies the speed of the left side
|
||||
* of the robot drive.
|
||||
* @param rightSpeed A function that supplies the speed of the right side
|
||||
* 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.
|
||||
*/
|
||||
RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::RamseteController controller, units::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(units::volt_t, units::volt_t)> output,
|
||||
std::initializer_list<Subsystem*> 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
|
||||
* outputs (in volts) for the robot drive.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
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);
|
||||
|
||||
void Initialize() override;
|
||||
|
||||
void Execute() override;
|
||||
|
||||
void End(bool interrupted) override;
|
||||
|
||||
bool IsFinished() override;
|
||||
|
||||
private:
|
||||
frc::Trajectory m_trajectory;
|
||||
std::function<frc::Pose2d()> m_pose;
|
||||
frc::RamseteController m_controller;
|
||||
const units::volt_t m_ks;
|
||||
const units::unit_t<voltsecondspermeter> m_kv;
|
||||
const units::unit_t<voltsecondssquaredpermeter> m_ka;
|
||||
frc::DifferentialDriveKinematics m_kinematics;
|
||||
std::function<units::meters_per_second_t()> m_leftSpeed;
|
||||
std::function<units::meters_per_second_t()> m_rightSpeed;
|
||||
std::unique_ptr<frc2::PIDController> m_leftController;
|
||||
std::unique_ptr<frc2::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;
|
||||
|
||||
Timer m_timer;
|
||||
units::second_t m_prevTime;
|
||||
frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
|
||||
};
|
||||
} // namespace frc2
|
||||
Reference in New Issue
Block a user