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

Add feedforward components (#2045)

Browse Source 
Add helper classes for computing feedforwards with parameters supplied by the characterization tool.
This commit is contained in:
Oblarg
2019-11-09 23:16:42 -05:00
committed by Peter Johnson
parent 5f33d6af12
commit 7dc7c71b58
13 changed files with 457 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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<Pose2d> 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<Pose2d> 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(),

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

@@ -10,16 +10,10 @@
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::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<units::meters_per_second_t()> leftSpeed,
std::function<units::meters_per_second_t()> 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(

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

@@ -13,6 +13,7 @@
#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>
@@ -41,12 +42,6 @@ namespace frc2 {
* @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:
/**
@@ -63,11 +58,7 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
* 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<CommandBase, RamseteCommand> {
* @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::RamseteController controller,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::DifferentialDriveKinematics kinematics,
std::function<units::meters_per_second_t()> leftSpeed,
std::function<units::meters_per_second_t()> rightSpeed,
@@ -129,9 +119,7 @@ class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
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::SimpleMotorFeedforward<units::meters> m_feedforward;
frc::DifferentialDriveKinematics m_kinematics;
std::function<units::meters_per_second_t()> m_leftSpeed;
std::function<units::meters_per_second_t()> m_rightSpeed;

33
wpilibc/src/main/native/cpp/controller/ArmFeedforward.cpp Normal file
View File

@@ -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 <wpi/MathExtras.h>
using namespace frc;
using Angle = units::radians;
using Velocity = units::radians_per_second;
using Acceleration = units::compound_unit<units::radians_per_second,
units::inverse<units::second>>;
using kv_unit = units::compound_unit<units::volts,
units::inverse<units::radians_per_second>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
ArmFeedforward::ArmFeedforward(units::volt_t kS, units::volt_t kCos,
units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA)
: m_kS(kS), m_kCos(kCos), m_kV(kV), m_kA(kA) {}
units::volt_t ArmFeedforward::Calculate(
units::unit_t<Angle> angle, units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) {
return m_kS * wpi::sgn(velocity) + m_kCos * units::math::cos(angle) +
m_kV * velocity + m_kA * acceleration;
}

62
wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h Normal file
View File

@@ -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 <units/units.h>
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<units::radians_per_second,
units::inverse<units::second>>;
using kv_unit =
units::compound_unit<units::volts,
units::inverse<units::radians_per_second>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
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_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(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> angle,
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0));
private:
units::volt_t m_kS{0};
units::volt_t m_kCos{0};
units::unit_t<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
};
} // namespace frc

64
wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h Normal file
View File

@@ -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 <units/units.h>
#include <wpi/MathExtras.h>
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 Distance>
class ElevatorFeedforward {
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
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_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(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> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(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<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
};
} // namespace frc

60
wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h Normal file
View File

@@ -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 <units/units.h>
#include <wpi/MathExtras.h>
namespace frc {
/**
* A helper class that computes feedforward voltages for a simple
* permanent-magnet DC motor.
*/
template <class Distance>
class SimpleMotorFeedforward {
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
using ka_unit =
units::compound_unit<units::volts, units::inverse<Acceleration>>;
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_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(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> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) {
return m_kS * wpi::sgn(velocity) + m_kV * velocity + m_kA * acceleration;
}
private:
units::volt_t m_kS{0};
units::unit_t<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
};
} // namespace frc

3
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
View File

@@ -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<units::meters>(
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
DriveConstants::kDriveKinematics,
[this] {
return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate());

72
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java Normal file
View File

@@ -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);
}
}

69
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java Normal file
View File

@@ -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);
}
}

64
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java Normal file
View File

@@ -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);
}
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
View File

@@ -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,

6
wpiutil/src/main/native/include/wpi/MathExtras.h
View File

@@ -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 <typename T>
int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
} // namespace wpi
#endif