From 7dc7c71b588d8dcf460f49ea3b1958ccc332afc7 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Sat, 9 Nov 2019 23:16:42 -0500 Subject: [PATCH] Add feedforward components (#2045) Add helper classes for computing feedforwards with parameters supplied by the characterization tool. --- .../wpilibj2/command/RamseteCommand.java | 35 +++------ .../cpp/frc2/command/RamseteCommand.cpp | 25 ++----- .../include/frc2/command/RamseteCommand.h | 22 ++---- .../native/cpp/controller/ArmFeedforward.cpp | 33 +++++++++ .../include/frc/controller/ArmFeedforward.h | 62 ++++++++++++++++ .../frc/controller/ElevatorFeedforward.h | 64 +++++++++++++++++ .../frc/controller/SimpleMotorFeedforward.h | 60 ++++++++++++++++ .../RamseteCommand/cpp/RobotContainer.cpp | 3 +- .../wpilibj/controller/ArmFeedforward.java | 72 +++++++++++++++++++ .../controller/ElevatorFeedforward.java | 69 ++++++++++++++++++ .../controller/SimpleMotorFeedforward.java | 64 +++++++++++++++++ .../ramsetecommand/RobotContainer.java | 5 +- .../src/main/native/include/wpi/MathExtras.h | 6 ++ 13 files changed, 457 insertions(+), 63 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/controller/ArmFeedforward.cpp create mode 100644 wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h create mode 100644 wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h create mode 100644 wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index e59e81aa0a..833c905c65 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -14,6 +14,7 @@ import java.util.function.Supplier; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; @@ -45,9 +46,7 @@ public class RamseteCommand extends CommandBase { private final Trajectory m_trajectory; private final Supplier m_pose; private final RamseteController m_follower; - private final double m_ks; - private final double m_kv; - private final double m_ka; + private final SimpleMotorFeedforward m_feedforward; private final DifferentialDriveKinematics m_kinematics; private final DoubleSupplier m_leftSpeed; private final DoubleSupplier m_rightSpeed; @@ -67,12 +66,8 @@ public class RamseteCommand extends CommandBase { * @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 ksVolts Constant feedforward term for the robot drive. - * @param kvVoltSecondsPerMeter Velocity-proportional feedforward term for the robot - * drive. - * @param kaVoltSecondsSquaredPerMeter Acceleration-proportional feedforward term for the robot - * drive. + * @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 leftWheelSpeedMetersPerSecond A function that supplies the speed of the left side of * the robot drive. @@ -88,9 +83,7 @@ public class RamseteCommand extends CommandBase { public RamseteCommand(Trajectory trajectory, Supplier pose, RamseteController controller, - double ksVolts, - double kvVoltSecondsPerMeter, - double kaVoltSecondsSquaredPerMeter, + SimpleMotorFeedforward feedforward, DifferentialDriveKinematics kinematics, DoubleSupplier leftWheelSpeedMetersPerSecond, DoubleSupplier rightWheelSpeedMetersPerSecond, @@ -101,9 +94,7 @@ public class RamseteCommand extends CommandBase { m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand"); m_pose = requireNonNullParam(pose, "pose", "RamseteCommand"); m_follower = requireNonNullParam(controller, "controller", "RamseteCommand"); - m_ks = ksVolts; - m_kv = kvVoltSecondsPerMeter; - m_ka = kaVoltSecondsSquaredPerMeter; + m_feedforward = feedforward; m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond, "leftWheelSpeedMetersPerSecond", @@ -146,9 +137,7 @@ public class RamseteCommand extends CommandBase { m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand"); m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand"); - m_ks = 0; - m_kv = 0; - m_ka = 0; + m_feedforward = null; m_leftSpeed = null; m_rightSpeed = null; m_leftController = null; @@ -192,14 +181,12 @@ public class RamseteCommand extends CommandBase { if (m_usePID) { double leftFeedforward = - m_ks * Math.signum(leftSpeedSetpoint) - + m_kv * leftSpeedSetpoint - + m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt; + m_feedforward.calculate(leftSpeedSetpoint, + (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt); double rightFeedforward = - m_ks * Math.signum(rightSpeedSetpoint) - + m_kv * rightSpeedSetpoint - + m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt; + m_feedforward.calculate(rightSpeedSetpoint, + (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt); leftOutput = leftFeedforward + m_leftController.calculate(m_leftSpeed.getAsDouble(), diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index 7be6655bf7..c0ed8b7504 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -10,16 +10,10 @@ using namespace frc2; using namespace units; -template -int sgn(T val) { - return (T(0) < val) - (val < T(0)); -} - RamseteCommand::RamseteCommand( frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, volt_t ks, - units::unit_t kv, - units::unit_t ka, + frc::RamseteController controller, + frc::SimpleMotorFeedforward feedforward, frc::DifferentialDriveKinematics kinematics, std::function leftSpeed, std::function rightSpeed, @@ -29,9 +23,7 @@ RamseteCommand::RamseteCommand( : m_trajectory(trajectory), m_pose(pose), m_controller(controller), - m_ks(ks), - m_kv(kv), - m_ka(ka), + m_feedforward(feedforward), m_kinematics(kinematics), m_leftSpeed(leftSpeed), m_rightSpeed(rightSpeed), @@ -52,9 +44,6 @@ RamseteCommand::RamseteCommand( : 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) { @@ -84,12 +73,12 @@ void RamseteCommand::Execute() { if (m_usePID) { auto leftFeedforward = - m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left + - m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt; + m_feedforward.Calculate(targetWheelSpeeds.left, + (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; + m_feedforward.Calculate(targetWheelSpeeds.right, + (targetWheelSpeeds.right - m_prevSpeeds.right) / dt); auto leftOutput = volt_t(m_leftController->Calculate( diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h index 0478ef2edf..5d8477ff0d 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -41,12 +42,6 @@ namespace frc2 { * @see Trajectory */ class RamseteCommand : public CommandHelper { - using voltsecondspermeter = - units::compound_unit>; - using voltsecondssquaredpermeter = - units::compound_unit, - units::inverse>; public: /** @@ -63,11 +58,7 @@ class RamseteCommand : public CommandHelper { * 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 feedforward A component for calculating the feedforward for the 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. @@ -82,9 +73,8 @@ class RamseteCommand : public CommandHelper { * @param requirements The subsystems to require. */ RamseteCommand(frc::Trajectory trajectory, std::function pose, - frc::RamseteController controller, units::volt_t ks, - units::unit_t kv, - units::unit_t ka, + frc::RamseteController controller, + frc::SimpleMotorFeedforward feedforward, frc::DifferentialDriveKinematics kinematics, std::function leftSpeed, std::function rightSpeed, @@ -129,9 +119,7 @@ class RamseteCommand : public CommandHelper { frc::Trajectory m_trajectory; std::function m_pose; frc::RamseteController m_controller; - const units::volt_t m_ks; - const units::unit_t m_kv; - const units::unit_t m_ka; + frc::SimpleMotorFeedforward m_feedforward; frc::DifferentialDriveKinematics m_kinematics; std::function m_leftSpeed; std::function m_rightSpeed; diff --git a/wpilibc/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpilibc/src/main/native/cpp/controller/ArmFeedforward.cpp new file mode 100644 index 0000000000..25e72b37d5 --- /dev/null +++ b/wpilibc/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "frc/controller/ArmFeedforward.h" + +#include + +using namespace frc; + +using Angle = units::radians; +using Velocity = units::radians_per_second; +using Acceleration = units::compound_unit>; +using kv_unit = units::compound_unit>; +using ka_unit = + units::compound_unit>; + +ArmFeedforward::ArmFeedforward(units::volt_t kS, units::volt_t kCos, + units::unit_t kV, + units::unit_t kA) + : m_kS(kS), m_kCos(kCos), m_kV(kV), m_kA(kA) {} + +units::volt_t ArmFeedforward::Calculate( + units::unit_t angle, units::unit_t velocity, + units::unit_t acceleration) { + return m_kS * wpi::sgn(velocity) + m_kCos * units::math::cos(angle) + + m_kV * velocity + m_kA * acceleration; +} diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h new file mode 100644 index 0000000000..75bcd216b2 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +namespace frc { +/** + * A helper class that computes feedforward outputs for a simple arm (modeled as + * a motor acting against the force of gravity on a beam suspended at an angle). + */ +class ArmFeedforward { + using Angle = units::radians; + using Velocity = units::radians_per_second; + using Acceleration = units::compound_unit>; + using kv_unit = + units::compound_unit>; + using ka_unit = + units::compound_unit>; + + public: + ArmFeedforward() = default; + + /** + * Creates a new ArmFeedforward with the specified gains. + * + * @param kS The static gain, in volts. + * @param kCos The gravity gain, in volts. + * @param kV The velocity gain, in volt seconds per radian. + * @param kA The acceleration gain, in volt seconds^2 per radian. + */ + ArmFeedforward(units::volt_t kS, units::volt_t kCos, + units::unit_t kV, + units::unit_t kA = units::unit_t(0)); + + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param angle The angle setpoint, in radians. + * @param velocity The velocity setpoint, in radians per second. + * @param acceleration The acceleration setpoint, in radians per second^2. + * @return The computed feedforward, in volts. + */ + units::volt_t Calculate(units::unit_t angle, + units::unit_t velocity, + units::unit_t acceleration = + units::unit_t(0)); + + private: + units::volt_t m_kS{0}; + units::volt_t m_kCos{0}; + units::unit_t m_kV{0}; + units::unit_t m_kA{0}; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h new file mode 100644 index 0000000000..2c1e9e12c0 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +namespace frc { +/** + * A helper class that computes feedforward outputs for a simple elevator + * (modeled as a motor acting against the force of gravity). + */ +template +class ElevatorFeedforward { + using Velocity = + units::compound_unit>; + using Acceleration = + units::compound_unit>; + using kv_unit = units::compound_unit>; + using ka_unit = + units::compound_unit>; + + public: + ElevatorFeedforward() = default; + + /** + * Creates a new ElevatorFeedforward with the specified gains. + * + * @param kS The static gain, in volts. + * @param kG The gravity gain, in volts. + * @param kV The velocity gain, in volt seconds per distance. + * @param kA The acceleration gain, in volt seconds^2 per distance. + */ + ElevatorFeedforward(units::volt_t kS, units::volt_t kG, + units::unit_t kV, + units::unit_t kA = units::unit_t(0)) + : m_kS(kS), m_kG(kG), m_kV(kV), m_kA(kA) {} + + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param velocity The velocity setpoint, in distance per second. + * @param acceleration The acceleration setpoint, in distance per second^2. + * @return The computed feedforward, in volts. + */ + units::volt_t Calculate(units::unit_t velocity, + units::unit_t acceleration = + units::unit_t(0)) { + return m_kS * wpi::sgn(velocity) + m_kG + m_kV * velocity + + m_kA * acceleration; + } + + private: + units::volt_t m_kS{0}; + units::volt_t m_kG{0}; + units::unit_t m_kV{0}; + units::unit_t m_kA{0}; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h new file mode 100644 index 0000000000..8293fc9e91 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +namespace frc { +/** + * A helper class that computes feedforward voltages for a simple + * permanent-magnet DC motor. + */ +template +class SimpleMotorFeedforward { + using Velocity = + units::compound_unit>; + using Acceleration = + units::compound_unit>; + using kv_unit = units::compound_unit>; + using ka_unit = + units::compound_unit>; + + public: + SimpleMotorFeedforward() = default; + + /** + * Creates a new SimpleMotorFeedforward with the specified gains. + * + * @param kS The static gain, in volts. + * @param kV The velocity gain, in volt seconds per distance. + * @param kA The acceleration gain, in volt seconds^2 per distance. + */ + SimpleMotorFeedforward(units::volt_t kS, units::unit_t kV, + units::unit_t kA = units::unit_t(0)) + : m_kS(kS), m_kV(kV), m_kA(kA) {} + + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param velocity The velocity setpoint, in distance per second. + * @param acceleration The acceleration setpoint, in distance per second^2. + * @return The computed feedforward, in volts. + */ + units::volt_t Calculate(units::unit_t velocity, + units::unit_t acceleration = + units::unit_t(0)) { + return m_kS * wpi::sgn(velocity) + m_kV * velocity + m_kA * acceleration; + } + + private: + units::volt_t m_kS{0}; + units::unit_t m_kV{0}; + units::unit_t m_kA{0}; +}; +} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index 7b2e7c4fb4..9342f324cf 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -65,7 +65,8 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { exampleTrajectory, [this]() { return m_drive.GetPose(); }, frc::RamseteController(AutoConstants::kRamseteB, AutoConstants::kRamseteZeta), - DriveConstants::ks, DriveConstants::kv, DriveConstants::ka, + frc::SimpleMotorFeedforward( + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka), DriveConstants::kDriveKinematics, [this] { return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate()); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java new file mode 100644 index 0000000000..786e7e2f83 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +/** + * A helper class that computes feedforward outputs for a simple arm (modeled as a motor + * acting against the force of gravity on a beam suspended at an angle). + */ +public class ArmFeedforward { + private final double m_ks; + private final double m_kcos; + private final double m_kv; + private final double m_ka; + + /** + * Creates a new ArmFeedforward with the specified gains. Units of the gain values + * will dictate units of the computed feedforward. + * + * @param ks The static gain. + * @param kcos The gravity gain. + * @param kv The velocity gain. + * @param ka The acceleration gain. + */ + public ArmFeedforward(double ks, double kcos, double kv, double ka) { + m_ks = ks; + m_kcos = kcos; + m_kv = kv; + m_ka = ka; + } + + /** + * Creates a new ArmFeedforward with the specified gains. Acceleration gain is + * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. + * + * @param ks The static gain. + * @param kcos The gravity gain. + * @param kv The velocity gain. + */ + public ArmFeedforward(double ks, double kcos, double kv) { + this(ks, kcos, kv, 0); + } + + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param velocityRadPerSec The velocity setpoint. + * @param accelRadPerSecSquared The acceleration setpoint. + * @return The computed feedforward. + */ + public double calculate(double positionRadians, double velocityRadPerSec, + double accelRadPerSecSquared) { + return m_ks * Math.signum(velocityRadPerSec) + m_kcos * Math.cos(positionRadians) + + m_kv * velocityRadPerSec + + m_ka * accelRadPerSecSquared; + } + + /** + * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to + * be zero). + * + * @param velocity The velocity setpoint. + * @return The computed feedforward. + */ + public double calculate(double positionRadians, double velocity) { + return calculate(positionRadians, velocity, 0); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java new file mode 100644 index 0000000000..86fa801085 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +/** + * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor + * acting against the force of gravity). + */ +public class ElevatorFeedforward { + private final double m_ks; + private final double m_kg; + private final double m_kv; + private final double m_ka; + + /** + * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values + * will dictate units of the computed feedforward. + * + * @param ks The static gain. + * @param kg The gravity gain. + * @param kv The velocity gain. + * @param ka The acceleration gain. + */ + public ElevatorFeedforward(double ks, double kg, double kv, double ka) { + m_ks = ks; + m_kg = kg; + m_kv = kv; + m_ka = ka; + } + + /** + * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is + * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. + * + * @param ks The static gain. + * @param kg The gravity gain. + * @param kv The velocity gain. + */ + public ElevatorFeedforward(double ks, double kg, double kv) { + this(ks, kg, kv, 0); + } + + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param velocity The velocity setpoint. + * @param acceleration The acceleration setpoint. + * @return The computed feedforward. + */ + public double calculate(double velocity, double acceleration) { + return m_ks * Math.signum(velocity) + m_kg + m_kv * velocity + m_ka * acceleration; + } + + /** + * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to + * be zero). + * + * @param velocity The velocity setpoint. + * @return The computed feedforward. + */ + public double calculate(double velocity) { + return calculate(velocity, 0); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java new file mode 100644 index 0000000000..68d4ecbfe1 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +/** + * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. + */ +public class SimpleMotorFeedforward { + private final double m_ks; + private final double m_kv; + private final double m_ka; + + /** + * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values + * will dictate units of the computed feedforward. + * + * @param ks The static gain. + * @param kv The velocity gain. + * @param ka The acceleration gain. + */ + public SimpleMotorFeedforward(double ks, double kv, double ka) { + m_ks = ks; + m_kv = kv; + m_ka = ka; + } + + /** + * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is + * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. + * + * @param ks The static gain. + * @param kv The velocity gain. + */ + public SimpleMotorFeedforward(double ks, double kv) { + this(ks, kv, 0); + } + + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param velocity The velocity setpoint. + * @param acceleration The acceleration setpoint. + * @return The computed feedforward. + */ + public double calculate(double velocity, double acceleration) { + return m_ks * Math.signum(velocity) + m_kv * velocity + m_ka * acceleration; + } + + /** + * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to + * be zero). + * + * @param velocity The velocity setpoint. + * @return The computed feedforward. + */ + public double calculate(double velocity) { + return calculate(velocity, 0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index 75b4215525..565ea539cc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; @@ -115,9 +116,7 @@ public class RobotContainer { exampleTrajectory, m_robotDrive::getPose, new RamseteController(kRamseteB, kRamseteZeta), - ksVolts, - kvVoltSecondsPerMeter, - kaVoltSecondsSquaredPerMeter, + new SimpleMotorFeedforward(ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter), kDriveKinematics, m_robotDrive.getLeftEncoder()::getRate, m_robotDrive.getRightEncoder()::getRate, diff --git a/wpiutil/src/main/native/include/wpi/MathExtras.h b/wpiutil/src/main/native/include/wpi/MathExtras.h index 4dda4342c9..8d5fec490c 100644 --- a/wpiutil/src/main/native/include/wpi/MathExtras.h +++ b/wpiutil/src/main/native/include/wpi/MathExtras.h @@ -838,6 +838,12 @@ SaturatingMultiplyAdd(T X, T Y, T A, bool *ResultOverflowed = nullptr) { return SaturatingAdd(A, Product, &Overflowed); } +// Typesafe implementation of the signum function. Returns -1 if negative, 1 if positive, 0 if 0. +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + } // namespace wpi #endif