diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp index 27832a6df5d..af07ea8dd63 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp @@ -48,7 +48,7 @@ void Elevator::ReachGoal(units::meter_t goal) { auto pidOutput = m_controller.Calculate(m_encoder.GetDistance(), m_setpoint.position / 1_m); auto feedforwardOutput = - m_feedforward.Calculate(m_setpoint.velocity, next.velocity, 20_ms); + m_feedforward.Calculate(m_setpoint.velocity, next.velocity); m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput); diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h index d3aab419d63..a056c2dda0d 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h @@ -7,6 +7,7 @@ #include #include +#include #include #include diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h index e53b32ddaea..d808d64cc7c 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h @@ -28,5 +28,5 @@ class ShooterSubsystem : public frc2::PIDSubsystem { frc::PWMSparkMax m_shooterMotor; frc::PWMSparkMax m_feederMotor; frc::Encoder m_shooterEncoder; - frc::SimpleMotorFeedforward m_shooterFeedforward; + frc::SimpleMotorFeedforward m_shooterFeedforward; }; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h index b31155934ff..2e9b580bb2e 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h @@ -38,7 +38,7 @@ class Shooter : public frc2::SubsystemBase { frc::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0], ShooterConstants::kEncoderPorts[1], ShooterConstants::kEncoderReversed}; - frc::SimpleMotorFeedforward m_shooterFeedforward{ + frc::SimpleMotorFeedforward m_shooterFeedforward{ ShooterConstants::kS, ShooterConstants::kV}; frc::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h index 79f6d44b364..f5ba8e603a7 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h @@ -60,7 +60,7 @@ inline constexpr units::turns_per_second_t kShooterTolerance = 50_tps; inline constexpr double kP = 1.0; inline constexpr units::volt_t kS = 0.05_V; -inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed; +inline constexpr kv_unit_t kV = 12_V / kShooterFreeSpeed; inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / 1_tr; inline constexpr double kFeederSpeed = 0.5; diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h index 4f10c18277d..7bed4b058e6 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h @@ -48,6 +48,6 @@ class Shooter : public frc2::SubsystemBase { }, this}}; frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0}; - frc::SimpleMotorFeedforward m_shooterFeedforward{ + frc::SimpleMotorFeedforward m_shooterFeedforward{ constants::shooter::kS, constants::shooter::kV, constants::shooter::kA}; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java index 43beedc0a27..0ee307865c1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java @@ -4,6 +4,10 @@ package edu.wpi.first.wpilibj.examples.armbot.subsystems; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -41,7 +45,10 @@ public ArmSubsystem() { @Override public void useOutput(double output, TrapezoidProfile.State setpoint) { // Calculate the feedforward from the sepoint - double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + double feedforward = + m_feedforward + .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity)) + .in(Volts); // Add the feedforward to the PID output to get the motor output m_motor.setVoltage(output + feedforward); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java index e083484b5e6..ed533dff251 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java @@ -4,6 +4,10 @@ package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants; @@ -33,7 +37,10 @@ public ArmSubsystem() { @Override public void useState(TrapezoidProfile.State setpoint) { // Calculate the feedforward from the sepoint - double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + double feedforward = + m_feedforward + .calculate(Radians.of(setpoint.position), RadiansPerSecond.of(setpoint.velocity)) + .in(Volts); // Add the feedforward to the PID output to get the motor output m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index ac2423f4342..3b2765a9907 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -4,6 +4,9 @@ package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -110,7 +113,10 @@ public void reachGoal(double goal) { // With the setpoint value we run PID control like normal double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position); - double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.020); + double feedforwardOutput = + m_feedforward + .calculate(MetersPerSecond.of(m_setpoint.velocity), MetersPerSecond.of(next.velocity)) + .in(Volts); m_motor.setVoltage(pidOutput + feedforwardOutput); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java index ee37915d67a..1938c70fa4b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java @@ -4,6 +4,9 @@ package edu.wpi.first.wpilibj.examples.elevatorprofiledpid; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -51,6 +54,8 @@ public void teleopPeriodic() { // Run controller and update motor output m_motor.setVoltage( m_controller.calculate(m_encoder.getDistance()) - + m_feedforward.calculate(m_controller.getSetpoint().velocity)); + + m_feedforward + .calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)) + .in(Volts)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index d4f04fed26d..90378bd119a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -4,6 +4,9 @@ package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -101,7 +104,8 @@ public void reachGoal(double goal) { // With the setpoint value we run PID control like normal double pidOutput = m_controller.calculate(m_encoder.getDistance()); - double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity); + double feedforwardOutput = + m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts); m_motor.setVoltage(pidOutput + feedforwardOutput); } diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index fc2904c965f..96602dfcedf 100644 --- a/wpimath/algorithms.md +++ b/wpimath/algorithms.md @@ -1,5 +1,219 @@ # Algorithms +## Simple motor feedforward + +### Derivation + +For a simple DC motor with the model + +``` + dx/dt = −kᵥ/kₐ x + 1/kₐ u - kₛ/kₐ sgn(x), +``` + +where + +``` + A = −kᵥ/kₐ + B = 1/kₐ + c = -kₛ/kₐ sgn(x) + A_d = eᴬᵀ + B_d = A⁻¹(eᴬᵀ - I)B + dx/dt = Ax + Bu + c +``` + +Discretize the affine model. + +``` + dx/dt = Ax + Bu + c + dx/dt = Ax + B(u + B⁺c) + xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ +``` + +Solve for uₖ. + +``` + B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ +``` + +Substitute in B assuming sgn(x) is a constant for the duration of the step. + +``` + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kₛ/kₐ sgn(x))) + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kₛ/kₐ sgn(x)) + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x) +``` + +Simplify the model when kₐ = 0. + +Simplify A. + +``` + A = −kᵥ/kₐ +``` + +As kₐ approaches zero, A approaches -∞. + +``` + A = −∞ +``` + +Simplify A_d. + +``` + A_d = eᴬᵀ + A_d = exp(−∞) + A_d = 0 +``` + +Simplify B_d. + +``` + B_d = A⁻¹(eᴬᵀ - I)B + B_d = A⁻¹((0) - I)B + B_d = A⁻¹(-I)B + B_d = -A⁻¹B + B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ) + B_d = (kᵥ/kₐ)⁻¹(1/kₐ) + B_d = kₐ/kᵥ(1/kₐ) + B_d = 1/kᵥ +``` + +Substitute these into the feedforward equation. + +``` + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₛ sgn(x) + uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kₛ sgn(x) + uₖ = (1/kᵥ)⁺(xₖ₊₁) + kₛ sgn(x) + uₖ = kᵥxₖ₊₁ + kₛ sgn(x) + uₖ = kₛ sgn(x) + kᵥxₖ₊₁ +``` + +Simplify the model when ka ≠ 0 + +``` + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) +``` + +where + +``` + A = −kᵥ/kₐ + B = 1/kₐ + A_d = eᴬᵀ + B_d = A⁻¹(eᴬᵀ - I)B +``` + +## Elevator feedforward + +### Derivation + +For an elevator with the model + +``` + dx/dt = −kᵥ/kₐ x + 1/kₐ u - kg/kₐ - kₛ/kₐ sgn(x) +``` + +where + +``` + A = −kᵥ/kₐ + B = 1/kₐ + c = -(kg/kₐ + kₛ/kₐ sgn(x)) + A_d = eᴬᵀ + B_d = A⁻¹(eᴬᵀ - I)B + dx/dt = Ax + Bu + c +``` + +Discretize the affine model. + +``` + dx/dt = Ax + Bu + c + dx/dt = Ax + B(u + B⁺c) + xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + xₖ₊₁ = A_d xₖ + B_d uₖ + B_d B⁺cₖ +``` + +Solve for uₖ. + +``` + B_d uₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ +``` + +Substitute in B assuming sgn(x) is a constant for the duration of the step. + +``` + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − kₐ(-(kg/kₐ + kₛ/kₐ sgn(x))) + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kₐ(kg/kₐ + kₛ/kₐ sgn(x)) + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x) +``` + +Simplify the model when kₐ = 0. + +Simplify A. + +``` + A = −kᵥ/kₐ +``` + +As kₐ approaches zero, A approaches -∞. + +``` + A = −∞ +``` + +Simplify A_d. + +``` + A_d = eᴬᵀ + A_d = exp(−∞) + A_d = 0 +``` + +Simplify B_d. + +``` + B_d = A⁻¹(eᴬᵀ - I)B + B_d = A⁻¹((0) - I)B + B_d = A⁻¹(-I)B + B_d = -A⁻¹B + B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ) + B_d = (kᵥ/kₐ)⁻¹(1/kₐ) + B_d = kₐ/kᵥ(1/kₐ) + B_d = 1/kᵥ +``` + +Substitute these into the feedforward equation. + +``` + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + kg + kₛ sgn(x) + uₖ = (1/kᵥ)⁺(xₖ₊₁ − (0) xₖ) + kg + kₛ sgn(x) + uₖ = (1/kᵥ)⁺(xₖ₊₁) + kg + kₛ sgn(x) + uₖ = kᵥxₖ₊₁ + kg + kₛ sgn(x) + uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁ +``` + +Simplify the model when ka ≠ 0 + +``` + uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) +``` + +where + +``` + A = −kᵥ/kₐ + B = 1/kₐ + A_d = eᴬᵀ + B_d = A⁻¹(eᴬᵀ - I)B +``` + ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I ### Derivation diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java index 1498b67a62a..4a0866f8b3c 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java @@ -18,49 +18,51 @@ import us.hebi.quickbuf.RepeatedByte; public final class Controller { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1755, - "ChBjb250cm9sbGVyLnByb3RvEgl3cGkucHJvdG8iWAoWUHJvdG9idWZBcm1GZWVkZm9yd2FyZBIOCgJr" + - "cxgBIAEoAVICa3MSDgoCa2cYAiABKAFSAmtnEg4KAmt2GAMgASgBUgJrdhIOCgJrYRgEIAEoAVICa2Ei" + - "ngEKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVGZWVkZm9yd2FyZBIbCglrdl9saW5lYXIYASABKAFS" + - "CGt2TGluZWFyEhsKCWthX2xpbmVhchgCIAEoAVIIa2FMaW5lYXISHQoKa3ZfYW5ndWxhchgDIAEoAVIJ" + - "a3ZBbmd1bGFyEh0KCmthX2FuZ3VsYXIYBCABKAFSCWthQW5ndWxhciJdChtQcm90b2J1ZkVsZXZhdG9y" + - "RmVlZGZvcndhcmQSDgoCa3MYASABKAFSAmtzEg4KAmtnGAIgASgBUgJrZxIOCgJrdhgDIAEoAVICa3YS" + - "DgoCa2EYBCABKAFSAmthImAKHlByb3RvYnVmU2ltcGxlTW90b3JGZWVkZm9yd2FyZBIOCgJrcxgBIAEo" + - "AVICa3MSDgoCa3YYAiABKAFSAmt2Eg4KAmthGAMgASgBUgJrYRIOCgJkdBgEIAEoAVICZHQiUgomUHJv" + - "dG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsVm9sdGFnZXMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVy" + - "aWdodBgCIAEoAVIFcmlnaHRCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3RvSocJCgYSBAAAJQEKCAoB" + - "DBIDAAASCggKAQISAwIAEgoICgEIEgMEADEKCQoCCAESAwQAMQoKCgIEABIEBgALAQoKCgMEAAESAwYI" + - "HgoLCgQEAAIAEgMHAhAKDAoFBAACAAUSAwcCCAoMCgUEAAIAARIDBwkLCgwKBQQAAgADEgMHDg8KCwoE" + - "BAACARIDCAIQCgwKBQQAAgEFEgMIAggKDAoFBAACAQESAwgJCwoMCgUEAAIBAxIDCA4PCgsKBAQAAgIS" + - "AwkCEAoMCgUEAAICBRIDCQIICgwKBQQAAgIBEgMJCQsKDAoFBAACAgMSAwkODwoLCgQEAAIDEgMKAhAK" + - "DAoFBAACAwUSAwoCCAoMCgUEAAIDARIDCgkLCgwKBQQAAgMDEgMKDg8KCgoCBAESBA0AEgEKCgoDBAEB" + - "EgMNCCwKCwoEBAECABIDDgIXCgwKBQQBAgAFEgMOAggKDAoFBAECAAESAw4JEgoMCgUEAQIAAxIDDhUW" + - "CgsKBAQBAgESAw8CFwoMCgUEAQIBBRIDDwIICgwKBQQBAgEBEgMPCRIKDAoFBAECAQMSAw8VFgoLCgQE" + - "AQICEgMQAhgKDAoFBAECAgUSAxACCAoMCgUEAQICARIDEAkTCgwKBQQBAgIDEgMQFhcKCwoEBAECAxID" + - "EQIYCgwKBQQBAgMFEgMRAggKDAoFBAECAwESAxEJEwoMCgUEAQIDAxIDERYXCgoKAgQCEgQUABkBCgoK" + - "AwQCARIDFAgjCgsKBAQCAgASAxUCEAoMCgUEAgIABRIDFQIICgwKBQQCAgABEgMVCQsKDAoFBAICAAMS", - "AxUODwoLCgQEAgIBEgMWAhAKDAoFBAICAQUSAxYCCAoMCgUEAgIBARIDFgkLCgwKBQQCAgEDEgMWDg8K" + - "CwoEBAICAhIDFwIQCgwKBQQCAgIFEgMXAggKDAoFBAICAgESAxcJCwoMCgUEAgICAxIDFw4PCgsKBAQC" + - "AgMSAxgCEAoMCgUEAgIDBRIDGAIICgwKBQQCAgMBEgMYCQsKDAoFBAICAwMSAxgODwoKCgIEAxIEGwAg" + - "AQoKCgMEAwESAxsIJgoLCgQEAwIAEgMcAhAKDAoFBAMCAAUSAxwCCAoMCgUEAwIAARIDHAkLCgwKBQQD" + - "AgADEgMcDg8KCwoEBAMCARIDHQIQCgwKBQQDAgEFEgMdAggKDAoFBAMCAQESAx0JCwoMCgUEAwIBAxID" + - "HQ4PCgsKBAQDAgISAx4CEAoMCgUEAwICBRIDHgIICgwKBQQDAgIBEgMeCQsKDAoFBAMCAgMSAx4ODwoL" + - "CgQEAwIDEgMfAhAKDAoFBAMCAwUSAx8CCAoMCgUEAwIDARIDHwkLCgwKBQQDAgMDEgMfDg8KCgoCBAQS" + - "BCIAJQEKCgoDBAQBEgMiCC4KCwoEBAQCABIDIwISCgwKBQQEAgAFEgMjAggKDAoFBAQCAAESAyMJDQoM" + - "CgUEBAIAAxIDIxARCgsKBAQEAgESAyQCEwoMCgUEBAIBBRIDJAIICgwKBQQEAgEBEgMkCQ4KDAoFBAQC" + - "AQMSAyQREmIGcHJvdG8z"); + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1897, + "ChBjb250cm9sbGVyLnByb3RvEgl3cGkucHJvdG8iaAoWUHJvdG9idWZBcm1GZWVkZm9yd2FyZBIOCgJr" + + "cxgBIAEoAVICa3MSDgoCa2cYAiABKAFSAmtnEg4KAmt2GAMgASgBUgJrdhIOCgJrYRgEIAEoAVICa2ES" + + "DgoCZHQYBSABKAFSAmR0Ip4BCiRQcm90b2J1ZkRpZmZlcmVudGlhbERyaXZlRmVlZGZvcndhcmQSGwoJ" + + "a3ZfbGluZWFyGAEgASgBUghrdkxpbmVhchIbCglrYV9saW5lYXIYAiABKAFSCGthTGluZWFyEh0KCmt2" + + "X2FuZ3VsYXIYAyABKAFSCWt2QW5ndWxhchIdCgprYV9hbmd1bGFyGAQgASgBUglrYUFuZ3VsYXIibQob" + + "UHJvdG9idWZFbGV2YXRvckZlZWRmb3J3YXJkEg4KAmtzGAEgASgBUgJrcxIOCgJrZxgCIAEoAVICa2cS" + + "DgoCa3YYAyABKAFSAmt2Eg4KAmthGAQgASgBUgJrYRIOCgJkdBgFIAEoAVICZHQiYAoeUHJvdG9idWZT" + + "aW1wbGVNb3RvckZlZWRmb3J3YXJkEg4KAmtzGAEgASgBUgJrcxIOCgJrdhgCIAEoAVICa3YSDgoCa2EY" + + "AyABKAFSAmthEg4KAmR0GAQgASgBUgJkdCJSCiZQcm90b2J1ZkRpZmZlcmVudGlhbERyaXZlV2hlZWxW" + + "b2x0YWdlcxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodEIaChhlZHUud3Bp" + + "LmZpcnN0Lm1hdGgucHJvdG9K9QkKBhIEAAAnAQoICgEMEgMAABIKCAoBAhIDAgASCggKAQgSAwQAMQoJ" + + "CgIIARIDBAAxCgoKAgQAEgQGAAwBCgoKAwQAARIDBggeCgsKBAQAAgASAwcCEAoMCgUEAAIABRIDBwII" + + "CgwKBQQAAgABEgMHCQsKDAoFBAACAAMSAwcODwoLCgQEAAIBEgMIAhAKDAoFBAACAQUSAwgCCAoMCgUE" + + "AAIBARIDCAkLCgwKBQQAAgEDEgMIDg8KCwoEBAACAhIDCQIQCgwKBQQAAgIFEgMJAggKDAoFBAACAgES" + + "AwkJCwoMCgUEAAICAxIDCQ4PCgsKBAQAAgMSAwoCEAoMCgUEAAIDBRIDCgIICgwKBQQAAgMBEgMKCQsK" + + "DAoFBAACAwMSAwoODwoLCgQEAAIEEgMLAhAKDAoFBAACBAUSAwsCCAoMCgUEAAIEARIDCwkLCgwKBQQA" + + "AgQDEgMLDg8KCgoCBAESBA4AEwEKCgoDBAEBEgMOCCwKCwoEBAECABIDDwIXCgwKBQQBAgAFEgMPAggK" + + "DAoFBAECAAESAw8JEgoMCgUEAQIAAxIDDxUWCgsKBAQBAgESAxACFwoMCgUEAQIBBRIDEAIICgwKBQQB" + + "AgEBEgMQCRIKDAoFBAECAQMSAxAVFgoLCgQEAQICEgMRAhgKDAoFBAECAgUSAxECCAoMCgUEAQICARID" + + "EQkTCgwKBQQBAgIDEgMRFhcKCwoEBAECAxIDEgIYCgwKBQQBAgMFEgMSAggKDAoFBAECAwESAxIJEwoM", + "CgUEAQIDAxIDEhYXCgoKAgQCEgQVABsBCgoKAwQCARIDFQgjCgsKBAQCAgASAxYCEAoMCgUEAgIABRID" + + "FgIICgwKBQQCAgABEgMWCQsKDAoFBAICAAMSAxYODwoLCgQEAgIBEgMXAhAKDAoFBAICAQUSAxcCCAoM" + + "CgUEAgIBARIDFwkLCgwKBQQCAgEDEgMXDg8KCwoEBAICAhIDGAIQCgwKBQQCAgIFEgMYAggKDAoFBAIC" + + "AgESAxgJCwoMCgUEAgICAxIDGA4PCgsKBAQCAgMSAxkCEAoMCgUEAgIDBRIDGQIICgwKBQQCAgMBEgMZ" + + "CQsKDAoFBAICAwMSAxkODwoLCgQEAgIEEgMaAhAKDAoFBAICBAUSAxoCCAoMCgUEAgIEARIDGgkLCgwK" + + "BQQCAgQDEgMaDg8KCgoCBAMSBB0AIgEKCgoDBAMBEgMdCCYKCwoEBAMCABIDHgIQCgwKBQQDAgAFEgMe" + + "AggKDAoFBAMCAAESAx4JCwoMCgUEAwIAAxIDHg4PCgsKBAQDAgESAx8CEAoMCgUEAwIBBRIDHwIICgwK" + + "BQQDAgEBEgMfCQsKDAoFBAMCAQMSAx8ODwoLCgQEAwICEgMgAhAKDAoFBAMCAgUSAyACCAoMCgUEAwIC" + + "ARIDIAkLCgwKBQQDAgIDEgMgDg8KCwoEBAMCAxIDIQIQCgwKBQQDAgMFEgMhAggKDAoFBAMCAwESAyEJ" + + "CwoMCgUEAwIDAxIDIQ4PCgoKAgQEEgQkACcBCgoKAwQEARIDJAguCgsKBAQEAgASAyUCEgoMCgUEBAIA" + + "BRIDJQIICgwKBQQEAgABEgMlCQ0KDAoFBAQCAAMSAyUQEQoLCgQEBAIBEgMmAhMKDAoFBAQCAQUSAyYC" + + "CAoMCgUEBAIBARIDJgkOCgwKBQQEAgEDEgMmERJiBnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("controller.proto", "wpi.proto", descriptorData); - static final Descriptors.Descriptor wpi_proto_ProtobufArmFeedforward_descriptor = descriptor.internalContainedType(31, 88, "ProtobufArmFeedforward", "wpi.proto.ProtobufArmFeedforward"); + static final Descriptors.Descriptor wpi_proto_ProtobufArmFeedforward_descriptor = descriptor.internalContainedType(31, 104, "ProtobufArmFeedforward", "wpi.proto.ProtobufArmFeedforward"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveFeedforward_descriptor = descriptor.internalContainedType(122, 158, "ProtobufDifferentialDriveFeedforward", "wpi.proto.ProtobufDifferentialDriveFeedforward"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveFeedforward_descriptor = descriptor.internalContainedType(138, 158, "ProtobufDifferentialDriveFeedforward", "wpi.proto.ProtobufDifferentialDriveFeedforward"); - static final Descriptors.Descriptor wpi_proto_ProtobufElevatorFeedforward_descriptor = descriptor.internalContainedType(282, 93, "ProtobufElevatorFeedforward", "wpi.proto.ProtobufElevatorFeedforward"); + static final Descriptors.Descriptor wpi_proto_ProtobufElevatorFeedforward_descriptor = descriptor.internalContainedType(298, 109, "ProtobufElevatorFeedforward", "wpi.proto.ProtobufElevatorFeedforward"); - static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(377, 96, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward"); + static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(409, 96, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(475, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(507, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages"); /** * @return this proto file's descriptor. @@ -95,6 +97,11 @@ public static final class ProtobufArmFeedforward extends ProtoMessageoptional double dt = 5; + */ + private double dt; + private ProtobufArmFeedforward() { } @@ -253,6 +260,43 @@ public ProtobufArmFeedforward setKa(final double value) { return this; } + /** + * optional double dt = 5; + * @return whether the dt field is set + */ + public boolean hasDt() { + return (bitField0_ & 0x00000010) != 0; + } + + /** + * optional double dt = 5; + * @return this + */ + public ProtobufArmFeedforward clearDt() { + bitField0_ &= ~0x00000010; + dt = 0D; + return this; + } + + /** + * optional double dt = 5; + * @return the dt + */ + public double getDt() { + return dt; + } + + /** + * optional double dt = 5; + * @param value the dt to set + * @return this + */ + public ProtobufArmFeedforward setDt(final double value) { + bitField0_ |= 0x00000010; + dt = value; + return this; + } + @Override public ProtobufArmFeedforward copyFrom(final ProtobufArmFeedforward other) { cachedSize = other.cachedSize; @@ -262,6 +306,7 @@ public ProtobufArmFeedforward copyFrom(final ProtobufArmFeedforward other) { kg = other.kg; kv = other.kv; ka = other.ka; + dt = other.dt; } return this; } @@ -284,6 +329,9 @@ public ProtobufArmFeedforward mergeFrom(final ProtobufArmFeedforward other) { if (other.hasKa()) { setKa(other.ka); } + if (other.hasDt()) { + setDt(other.dt); + } return this; } @@ -298,6 +346,7 @@ public ProtobufArmFeedforward clear() { kg = 0D; kv = 0D; ka = 0D; + dt = 0D; return this; } @@ -324,7 +373,8 @@ public boolean equals(Object o) { && (!hasKs() || ProtoUtil.isEqual(ks, other.ks)) && (!hasKg() || ProtoUtil.isEqual(kg, other.kg)) && (!hasKv() || ProtoUtil.isEqual(kv, other.kv)) - && (!hasKa() || ProtoUtil.isEqual(ka, other.ka)); + && (!hasKa() || ProtoUtil.isEqual(ka, other.ka)) + && (!hasDt() || ProtoUtil.isEqual(dt, other.dt)); } @Override @@ -345,6 +395,10 @@ public void writeTo(final ProtoSink output) throws IOException { output.writeRawByte((byte) 33); output.writeDoubleNoTag(ka); } + if ((bitField0_ & 0x00000010) != 0) { + output.writeRawByte((byte) 41); + output.writeDoubleNoTag(dt); + } } @Override @@ -362,6 +416,9 @@ protected int computeSerializedSize() { if ((bitField0_ & 0x00000008) != 0) { size += 9; } + if ((bitField0_ & 0x00000010) != 0) { + size += 9; + } return size; } @@ -404,6 +461,15 @@ public ProtobufArmFeedforward mergeFrom(final ProtoSource input) throws IOExcept ka = input.readDouble(); bitField0_ |= 0x00000008; tag = input.readTag(); + if (tag != 41) { + break; + } + } + case 41: { + // dt + dt = input.readDouble(); + bitField0_ |= 0x00000010; + tag = input.readTag(); if (tag != 0) { break; } @@ -437,6 +503,9 @@ public void writeTo(final JsonSink output) throws IOException { if ((bitField0_ & 0x00000008) != 0) { output.writeDouble(FieldNames.ka, ka); } + if ((bitField0_ & 0x00000010) != 0) { + output.writeDouble(FieldNames.dt, dt); + } output.endObject(); } @@ -491,6 +560,17 @@ public ProtobufArmFeedforward mergeFrom(final JsonSource input) throws IOExcepti } break; } + case 3216: { + if (input.isAtField(FieldNames.dt)) { + if (!input.trySkipNullValue()) { + dt = input.readDouble(); + bitField0_ |= 0x00000010; + } + } else { + input.skipUnknownField(); + } + break; + } default: { input.skipUnknownField(); break; @@ -558,6 +638,8 @@ static class FieldNames { static final FieldName kv = FieldName.forField("kv"); static final FieldName ka = FieldName.forField("ka"); + + static final FieldName dt = FieldName.forField("dt"); } } @@ -1089,6 +1171,11 @@ public static final class ProtobufElevatorFeedforward extends ProtoMessageoptional double dt = 5; + */ + private double dt; + private ProtobufElevatorFeedforward() { } @@ -1247,6 +1334,43 @@ public ProtobufElevatorFeedforward setKa(final double value) { return this; } + /** + * optional double dt = 5; + * @return whether the dt field is set + */ + public boolean hasDt() { + return (bitField0_ & 0x00000010) != 0; + } + + /** + * optional double dt = 5; + * @return this + */ + public ProtobufElevatorFeedforward clearDt() { + bitField0_ &= ~0x00000010; + dt = 0D; + return this; + } + + /** + * optional double dt = 5; + * @return the dt + */ + public double getDt() { + return dt; + } + + /** + * optional double dt = 5; + * @param value the dt to set + * @return this + */ + public ProtobufElevatorFeedforward setDt(final double value) { + bitField0_ |= 0x00000010; + dt = value; + return this; + } + @Override public ProtobufElevatorFeedforward copyFrom(final ProtobufElevatorFeedforward other) { cachedSize = other.cachedSize; @@ -1256,6 +1380,7 @@ public ProtobufElevatorFeedforward copyFrom(final ProtobufElevatorFeedforward ot kg = other.kg; kv = other.kv; ka = other.ka; + dt = other.dt; } return this; } @@ -1278,6 +1403,9 @@ public ProtobufElevatorFeedforward mergeFrom(final ProtobufElevatorFeedforward o if (other.hasKa()) { setKa(other.ka); } + if (other.hasDt()) { + setDt(other.dt); + } return this; } @@ -1292,6 +1420,7 @@ public ProtobufElevatorFeedforward clear() { kg = 0D; kv = 0D; ka = 0D; + dt = 0D; return this; } @@ -1318,7 +1447,8 @@ public boolean equals(Object o) { && (!hasKs() || ProtoUtil.isEqual(ks, other.ks)) && (!hasKg() || ProtoUtil.isEqual(kg, other.kg)) && (!hasKv() || ProtoUtil.isEqual(kv, other.kv)) - && (!hasKa() || ProtoUtil.isEqual(ka, other.ka)); + && (!hasKa() || ProtoUtil.isEqual(ka, other.ka)) + && (!hasDt() || ProtoUtil.isEqual(dt, other.dt)); } @Override @@ -1339,6 +1469,10 @@ public void writeTo(final ProtoSink output) throws IOException { output.writeRawByte((byte) 33); output.writeDoubleNoTag(ka); } + if ((bitField0_ & 0x00000010) != 0) { + output.writeRawByte((byte) 41); + output.writeDoubleNoTag(dt); + } } @Override @@ -1356,6 +1490,9 @@ protected int computeSerializedSize() { if ((bitField0_ & 0x00000008) != 0) { size += 9; } + if ((bitField0_ & 0x00000010) != 0) { + size += 9; + } return size; } @@ -1398,6 +1535,15 @@ public ProtobufElevatorFeedforward mergeFrom(final ProtoSource input) throws IOE ka = input.readDouble(); bitField0_ |= 0x00000008; tag = input.readTag(); + if (tag != 41) { + break; + } + } + case 41: { + // dt + dt = input.readDouble(); + bitField0_ |= 0x00000010; + tag = input.readTag(); if (tag != 0) { break; } @@ -1431,6 +1577,9 @@ public void writeTo(final JsonSink output) throws IOException { if ((bitField0_ & 0x00000008) != 0) { output.writeDouble(FieldNames.ka, ka); } + if ((bitField0_ & 0x00000010) != 0) { + output.writeDouble(FieldNames.dt, dt); + } output.endObject(); } @@ -1485,6 +1634,17 @@ public ProtobufElevatorFeedforward mergeFrom(final JsonSource input) throws IOEx } break; } + case 3216: { + if (input.isAtField(FieldNames.dt)) { + if (!input.trySkipNullValue()) { + dt = input.readDouble(); + bitField0_ |= 0x00000010; + } + } else { + input.skipUnknownField(); + } + break; + } default: { input.skipUnknownField(); break; @@ -1553,6 +1713,8 @@ static class FieldNames { static final FieldName kv = FieldName.forField("kv"); static final FieldName ka = FieldName.forField("ka"); + + static final FieldName dt = FieldName.forField("dt"); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index cd66696e502..61eb2c5932c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -4,9 +4,17 @@ package edu.wpi.first.math.controller; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.proto.ArmFeedforwardProto; import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct; import edu.wpi.first.math.jni.ArmFeedforwardJNI; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -27,24 +35,25 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable /** The acceleration gain, in V/(rad/s²). */ private final double ka; - /** Arm feedforward protobuf for serialization. */ - public static final ArmFeedforwardProto proto = new ArmFeedforwardProto(); + /** The period, in seconds. */ + private final double m_dt; - /** Arm feedforward struct for serialization. */ - public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct(); + /** The calculated output voltage measure. */ + private final MutVoltage output = Volts.mutable(0.0); /** - * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate - * units of the computed feedforward. + * Creates a new ArmFeedforward with the specified gains and period. * - * @param ks The static gain. - * @param kg The gravity gain. - * @param kv The velocity gain. - * @param ka The acceleration gain. + * @param ks The static gain in volts. + * @param kg The gravity gain in volts. + * @param kv The velocity gain in V/(rad/s). + * @param ka The acceleration gain in V/(rad/s²). + * @param dtSeconds The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. + * @throws IllegalArgumentException for period ≤ zero. */ - public ArmFeedforward(double ks, double kg, double kv, double ka) { + public ArmFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) { this.ks = ks; this.kg = kg; this.kv = kv; @@ -55,56 +64,84 @@ public ArmFeedforward(double ks, double kg, double kv, double ka) { if (ka < 0.0) { throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } + if (dtSeconds <= 0.0) { + throw new IllegalArgumentException( + "period must be a positive number, got " + dtSeconds + "!"); + } + m_dt = dtSeconds; } /** - * 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. + * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms. * - * @param ks The static gain. - * @param kg The gravity gain. - * @param kv The velocity gain. + * @param ks The static gain in volts. + * @param kg The gravity gain in volts. + * @param kv The velocity gain in V/(rad/s). + * @param ka The acceleration gain in V/(rad/s²). + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. + */ + public ArmFeedforward(double ks, double kg, double kv, double ka) { + this(ks, kg, kv, ka, 0.020); + } + + /** + * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms. + * + * @param ks The static gain in volts. + * @param kg The gravity gain in volts. + * @param kv The velocity gain in V/(rad/s). + * @throws IllegalArgumentException for kv < zero. */ public ArmFeedforward(double ks, double kg, double kv) { this(ks, kg, kv, 0); } /** - * Returns the static gain. + * Returns the static gain in volts. * - * @return The static gain, in volts. + * @return The static gain in volts. */ public double getKs() { return ks; } /** - * Returns the gravity gain. + * Returns the gravity gain in volts. * - * @return The gravity gain, in volts. + * @return The gravity gain in volts. */ public double getKg() { return kg; } /** - * Returns the velocity gain. + * Returns the velocity gain in V/(rad/s). * - * @return The velocity gain, in V/(rad/s). + * @return The velocity gain. */ public double getKv() { return kv; } /** - * Returns the acceleration gain. + * Returns the acceleration gain in V/(rad/s²). * - * @return The acceleration gain, in V/(rad/s²). + * @return The acceleration gain. */ public double getKa() { return ka; } + /** + * Returns the period in seconds. + * + * @return The period in seconds. + */ + public double getDt() { + return m_dt; + } + /** * Calculates the feedforward from the gains and setpoints. * @@ -115,6 +152,7 @@ public double getKa() { * @param accelRadPerSecSquared The acceleration setpoint. * @return The computed feedforward. */ + @Deprecated(forRemoval = true, since = "2025") public double calculate( double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) { return ks * Math.signum(velocityRadPerSec) @@ -124,8 +162,8 @@ public double calculate( } /** - * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be - * zero). + * Calculates the feedforward from the gains and velocity setpoint assuming continuous control + * (acceleration is assumed to be zero). * * @param positionRadians The position (angle) setpoint. This angle should be measured from the * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If @@ -133,12 +171,14 @@ public double calculate( * @param velocity The velocity setpoint. * @return The computed feedforward. */ + @SuppressWarnings("removal") + @Deprecated(forRemoval = true, since = "2025") public double calculate(double positionRadians, double velocity) { return calculate(positionRadians, velocity, 0); } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous control. * * @param currentAngle The current angle in radians. This angle should be measured from the * horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If @@ -148,12 +188,59 @@ public double calculate(double positionRadians, double velocity) { * @param dt Time between velocity setpoints in seconds. * @return The computed feedforward in volts. */ + @SuppressWarnings("removal") + @Deprecated(forRemoval = true, since = "2025") public double calculate( double currentAngle, double currentVelocity, double nextVelocity, double dt) { return ArmFeedforwardJNI.calculate( ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt); } + /** + * Calculates the feedforward from the gains and setpoints assuming discrete control when the + * velocity does not change. + * + * @param currentAngle The current angle. This angle should be measured from the horizontal (i.e. + * if the provided angle is 0, the arm should be parallel to the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param currentVelocity The current velocity. + * @return The computed feedforward in volts. + */ + public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) { + output.mut_replace( + kg * Math.cos(currentAngle.in(Radians)) + + ks * Math.signum(currentVelocity.in(RadiansPerSecond)) + + kv * currentVelocity.in(RadiansPerSecond), + Volts); + return output; + } + + /** + * Calculates the feedforward from the gains and setpoints assuming discrete control. + * + * @param currentAngle The current angle. This angle should be measured from the horizontal (i.e. + * if the provided angle is 0, the arm should be parallel to the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. + * @return The computed feedforward in volts. + */ + public Voltage calculate( + Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) { + output.mut_replace( + ArmFeedforwardJNI.calculate( + ks, + kv, + ka, + kg, + currentAngle.in(Radians), + currentVelocity.in(RadiansPerSecond), + nextVelocity.in(RadiansPerSecond), + m_dt), + Volts); + return output; + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: @@ -164,11 +251,11 @@ public double calculate( * you a simultaneously-achievable velocity constraint. * * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param acceleration The acceleration of the arm. - * @return The maximum possible velocity at the given acceleration and angle. + * @param angle The angle of the arm, in radians. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param acceleration The acceleration of the arm, in (rad/s²). + * @return The maximum possible velocity in (rad/s) at the given acceleration and angle. */ public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) { // Assume max velocity is positive @@ -181,12 +268,12 @@ public double maxAchievableVelocity(double maxVoltage, double angle, double acce * profile are simultaneously achievable - enter the acceleration constraint, and this will give * you a simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param acceleration The acceleration of the arm. - * @return The minimum possible velocity at the given acceleration and angle. + * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts. + * @param angle The angle of the arm, in radians. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param acceleration The acceleration of the arm, in (rad/s²). + * @return The minimum possible velocity in (rad/s) at the given acceleration and angle. */ public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) { // Assume min velocity is negative, ks flips sign @@ -199,12 +286,12 @@ public double minAchievableVelocity(double maxVoltage, double angle, double acce * profile are simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param velocity The velocity of the arm. - * @return The maximum possible acceleration at the given velocity. + * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts. + * @param angle The angle of the arm, in radians. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param velocity The velocity of the elevator, in (rad/s) + * @return The maximum possible acceleration in (rad/s²) at the given velocity. */ public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka; @@ -216,14 +303,20 @@ public double maxAchievableAcceleration(double maxVoltage, double angle, double * profile are simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param velocity The velocity of the arm. - * @return The minimum possible acceleration at the given velocity. + * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts. + * @param angle The angle of the arm, in radians. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param velocity The velocity of the elevator, in (rad/s) + * @return The maximum possible acceleration in (rad/s²) at the given velocity. */ public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { return maxAchievableAcceleration(-maxVoltage, angle, velocity); } + + /** Arm feedforward struct for serialization. */ + public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct(); + + /** Arm feedforward protobuf for serialization. */ + public static final ArmFeedforwardProto proto = new ArmFeedforwardProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 0c943979c0d..8445e509c28 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -4,11 +4,14 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto; import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct; -import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -17,36 +20,37 @@ * against the force of gravity). */ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable { - /** The static gain. */ + /** The static gain, in volts. */ private final double ks; - /** The gravity gain. */ + /** The gravity gain, in volts. */ private final double kg; - /** The velocity gain. */ + /** The velocity gain, in V/(m/s). */ private final double kv; - /** The acceleration gain. */ + /** The acceleration gain, in V/(m/s²). */ private final double ka; - /** ElevatorFeedforward protobuf for serialization. */ - public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto(); + /** The period, in seconds. */ + private final double m_dt; - /** ElevatorFeedforward struct for serialization. */ - public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct(); + /** The calculated output voltage measure. */ + private final MutVoltage output = Volts.mutable(0.0); /** - * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will - * dictate units of the computed feedforward. + * Creates a new ElevatorFeedforward with the specified gains and period. * - * @param ks The static gain. - * @param kg The gravity gain. - * @param kv The velocity gain. - * @param ka The acceleration gain. + * @param ks The static gain in volts. + * @param kg The gravity gain in volts. + * @param kv The velocity gain in V/(m/s). + * @param ka The acceleration gain in V/(m/s²). + * @param dtSeconds The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. + * @throws IllegalArgumentException for period ≤ zero. */ - public ElevatorFeedforward(double ks, double kg, double kv, double ka) { + public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) { this.ks = ks; this.kg = kg; this.kv = kv; @@ -57,40 +61,60 @@ public ElevatorFeedforward(double ks, double kg, double kv, double ka) { if (ka < 0.0) { throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } + if (dtSeconds <= 0.0) { + throw new IllegalArgumentException( + "period must be a positive number, got " + dtSeconds + "!"); + } + m_dt = dtSeconds; + } + + /** + * Creates a new ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms. + * + * @param ks The static gain in volts. + * @param kg The gravity gain in volts. + * @param kv The velocity gain in V/(m/s). + * @param ka The acceleration gain in V/(m/s²). + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. + */ + public ElevatorFeedforward(double ks, double kg, double kv, double ka) { + this(ks, kg, kv, ka, 0.020); } /** * 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. + * zero. The period is defaulted to 20 ms. * - * @param ks The static gain. - * @param kg The gravity gain. - * @param kv The velocity gain. + * @param ks The static gain in volts. + * @param kg The gravity gain in volts. + * @param kv The velocity gain in V/(m/s). + * @throws IllegalArgumentException for kv < zero. */ public ElevatorFeedforward(double ks, double kg, double kv) { this(ks, kg, kv, 0); } /** - * Returns the static gain. + * Returns the static gain in volts. * - * @return The static gain. + * @return The static gain in volts. */ public double getKs() { return ks; } /** - * Returns the gravity gain. + * Returns the gravity gain in volts. * - * @return The gravity gain. + * @return The gravity gain in volts. */ public double getKg() { return kg; } /** - * Returns the velocity gain. + * Returns the velocity gain in V/(m/s). * * @return The velocity gain. */ @@ -99,7 +123,7 @@ public double getKv() { } /** - * Returns the acceleration gain. + * Returns the acceleration gain in V/(m/s²). * * @return The acceleration gain. */ @@ -108,67 +132,83 @@ public double getKa() { } /** - * Calculates the feedforward from the gains and setpoints. + * Returns the period in seconds. + * + * @return The period in seconds. + */ + public double getDt() { + return m_dt; + } + + /** + * Calculates the feedforward from the gains and setpoints assuming continuous control. * * @param velocity The velocity setpoint. * @param acceleration The acceleration setpoint. * @return The computed feedforward. */ + @SuppressWarnings("removal") + @Deprecated(forRemoval = true, since = "2025") public double calculate(double velocity, double acceleration) { return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and velocity setpoint assuming continuous control + * (acceleration is assumed to be zero). * - *

Note this method is inaccurate when the velocity crosses 0. + * @param velocity The velocity setpoint. + * @return The computed feedforward. + */ + @SuppressWarnings("removal") + @Deprecated(forRemoval = true, since = "2025") + public double calculate(double velocity) { + return calculate(velocity, 0); + } + + /** + * Calculates the feedforward from the gains and setpoints assuming discrete control when the + * setpoint does not change. * - * @param currentVelocity The current velocity setpoint. - * @param nextVelocity The next velocity setpoint. - * @param dtSeconds Time between velocity setpoints in seconds. + * @param currentVelocity The velocity setpoint. * @return The computed feedforward. */ - public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { - // Discretize the affine model. - // - // dx/dt = Ax + Bu + c - // dx/dt = Ax + B(u + B⁺c) - // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ - // - // Solve for uₖ. - // - // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ - // - // For an elevator with the model - // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), - // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B - // assuming sgn(x) is a constant for the duration of the step. - // - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) - var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); - var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); - - var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity); - var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity); - - return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); - } - - /** - * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be - * zero). + public Voltage calculate(LinearVelocity currentVelocity) { + return calculate(currentVelocity, currentVelocity); + } + + /** + * Calculates the feedforward from the gains and setpoints assuming discrete control. * - * @param velocity The velocity setpoint. + *

Note this method is inaccurate when the velocity crosses 0. + * + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. * @return The computed feedforward. */ - public double calculate(double velocity) { - return calculate(velocity, 0); + public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) { + // See wpimath/algorithms.md#Elevator_feedforward for derivation + if (ka == 0.0) { + output.mut_replace( + ks * Math.signum(nextVelocity.in(MetersPerSecond)) + + kg + + kv * nextVelocity.in(MetersPerSecond), + Volts); + return output; + } else { + double A = -kv / ka; + double B = 1.0 / ka; + double A_d = Math.exp(A * m_dt); + double B_d = 1.0 / A * (A_d - 1.0) * B; + output.mut_replace( + kg + + ks * Math.signum(currentVelocity.magnitude()) + + 1.0 + / B_d + * (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)), + Volts); + return output; + } } // Rearranging the main equation from the calculate() method yields the @@ -180,9 +220,9 @@ public double calculate(double velocity) { * simultaneously achievable - enter the acceleration constraint, and this will give you a * simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the elevator. - * @param acceleration The acceleration of the elevator. - * @return The maximum possible velocity at the given acceleration. + * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. + * @param acceleration The acceleration of the elevator, in (m/s²). + * @return The maximum possible velocity in (m/s) at the given acceleration. */ public double maxAchievableVelocity(double maxVoltage, double acceleration) { // Assume max velocity is positive @@ -195,9 +235,9 @@ public double maxAchievableVelocity(double maxVoltage, double acceleration) { * simultaneously achievable - enter the acceleration constraint, and this will give you a * simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the elevator. - * @param acceleration The acceleration of the elevator. - * @return The minimum possible velocity at the given acceleration. + * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. + * @param acceleration The acceleration of the elevator, in (m/s²). + * @return The maximum possible velocity in (m/s) at the given acceleration. */ public double minAchievableVelocity(double maxVoltage, double acceleration) { // Assume min velocity is negative, ks flips sign @@ -210,9 +250,9 @@ public double minAchievableVelocity(double maxVoltage, double acceleration) { * simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the elevator. - * @param velocity The velocity of the elevator. - * @return The maximum possible acceleration at the given velocity. + * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. + * @param velocity The velocity of the elevator, in (m/s) + * @return The maximum possible acceleration in (m/s²) at the given velocity. */ public double maxAchievableAcceleration(double maxVoltage, double velocity) { return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka; @@ -224,11 +264,17 @@ public double maxAchievableAcceleration(double maxVoltage, double velocity) { * simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the elevator. - * @param velocity The velocity of the elevator. - * @return The minimum possible acceleration at the given velocity. + * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. + * @param velocity The velocity of the elevator, in (m/s) + * @return The maximum possible acceleration in (m/s²) at the given velocity. */ public double minAchievableAcceleration(double maxVoltage, double velocity) { return maxAchievableAcceleration(-maxVoltage, velocity); } + + /** ElevatorFeedforward struct for serialization. */ + public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct(); + + /** ElevatorFeedforward protobuf for serialization. */ + public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index eb8fae85594..e227a63f5e6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -12,31 +12,36 @@ import edu.wpi.first.units.PerUnit; import edu.wpi.first.units.TimeUnit; import edu.wpi.first.units.Unit; +import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable { - /** The static gain. */ + /** The static gain, in volts. */ private final double ks; - /** The velocity gain. */ + /** The velocity gain, in V/(units/s). */ private final double kv; - /** The acceleration gain. */ + /** The acceleration gain, in V/(units/s²). */ private final double ka; - /** The period. */ + /** The period, in seconds. */ private final double m_dt; + // ** The calculated output voltage measure */ + private final MutVoltage output = Volts.mutable(0.0); + /** - * Creates a new SimpleMotorFeedforward with the specified gains and period. Units of the gain - * values will dictate units of the computed feedforward. + * Creates a new SimpleMotorFeedforward with the specified gains and period. + * + *

The units should be radians for angular systems and meters for linear systems. * - * @param ks The static gain. - * @param kv The velocity gain. - * @param ka The acceleration gain. + * @param ks The static gain in volts. + * @param kv The velocity gain in V/(units/s). + * @param ka The acceleration gain in V/(units/s²). * @param dtSeconds The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. @@ -61,11 +66,13 @@ public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) /** * Creates a new SimpleMotorFeedforward with the specified gains and period. The period is - * defaulted to 20 ms. Units of the gain values will dictate units of the computed feedforward. + * defaulted to 20 ms. + * + *

The units should be radians for angular systems and meters for linear systems. * - * @param ks The static gain. - * @param kv The velocity gain. - * @param ka The acceleration gain. + * @param ks The static gain in volts. + * @param kv The velocity gain in V/(units/s). + * @param ka The acceleration gain in V/(units/s²). * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. */ @@ -75,45 +82,51 @@ public SimpleMotorFeedforward(double ks, double kv, double ka) { /** * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted - * to zero. The period is defaulted to 20 ms. Units of the gain values will dictate units of the - * computed feedforward. + * to zero. The period is defaulted to 20 ms. * - * @param ks The static gain. - * @param kv The velocity gain. + *

The units should be radians for angular systems and meters for linear systems. + * + * @param ks The static gain in volts. + * @param kv The velocity gain in V/(units/s). + * @throws IllegalArgumentException for kv < zero. */ public SimpleMotorFeedforward(double ks, double kv) { - this(ks, kv, 0, 0.020); + this(ks, kv, 0); } /** - * Returns the static gain. + * Returns the static gain in volts. * - * @return The static gain. + * @return The static gain in volts. */ public double getKs() { return ks; } /** - * Returns the velocity gain. + * Returns the velocity gain in V/(units/s). + * + *

The units should be radians for angular systems and meters for linear systems. * - * @return The velocity gain. + * @return The velocity gain in V/(units/s). */ public double getKv() { return kv; } /** - * Returns the acceleration gain. + * Returns the acceleration gain in V/(units/s²). * - * @return The acceleration gain. + *

The units should be radians for angular systems and meters for linear systems. + * + * @return The acceleration gain in V/(units/s²). */ public double getKa() { return ka; } /** - * Returns the period. + * Returns the period in seconds. * * @return The period in seconds. */ @@ -122,7 +135,7 @@ public double getDt() { } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous control. * * @param velocity The velocity setpoint. * @param acceleration The acceleration setpoint. @@ -136,8 +149,8 @@ public double calculate(double velocity, double acceleration) { } /** - * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be - * zero). + * Calculates the feedforward from the gains and velocity setpoint assuming continuous control + * (acceleration is assumed to be zero). * * @param velocity The velocity setpoint. * @return The computed feedforward. @@ -164,6 +177,8 @@ public Voltage calculate(Measure /** * Calculates the feedforward from the gains and setpoints assuming discrete control. * + *

Note this method is inaccurate when the velocity crosses 0. + * * @param The velocity parameter either as distance or angle. * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. @@ -172,67 +187,21 @@ public Voltage calculate(Measure public Voltage calculate( Measure> currentVelocity, Measure> nextVelocity) { + // See wpimath/algorithms.md#Simple_motor_feedforward for derivation if (ka == 0.0) { - // Given the following discrete feedforward model - // - // uₖ = B_d⁺(rₖ₊₁ − A_d rₖ) - // - // where - // - // A_d = eᴬᵀ - // B_d = A⁻¹(eᴬᵀ - I)B - // A = −kᵥ/kₐ - // B = 1/kₐ - // - // We want the feedforward model when kₐ = 0. - // - // Simplify A. - // - // A = −kᵥ/kₐ - // - // As kₐ approaches zero, A approaches -∞. - // - // A = −∞ - // - // Simplify A_d. - // - // A_d = eᴬᵀ - // A_d = exp(−∞) - // A_d = 0 - // - // Simplify B_d. - // - // B_d = A⁻¹(eᴬᵀ - I)B - // B_d = A⁻¹((0) - I)B - // B_d = A⁻¹(-I)B - // B_d = -A⁻¹B - // B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ) - // B_d = (kᵥ/kₐ)⁻¹(1/kₐ) - // B_d = kₐ/kᵥ(1/kₐ) - // B_d = 1/kᵥ - // - // Substitute these into the feedforward equation. - // - // uₖ = B_d⁺(rₖ₊₁ − A_d rₖ) - // uₖ = (1/kᵥ)⁺(rₖ₊₁ − (0) rₖ) - // uₖ = kᵥrₖ₊₁ - return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude()); + output.mut_replace( + ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude(), Volts); + return output; } else { - // uₖ = B_d⁺(rₖ₊₁ − A_d rₖ) - // - // where - // - // A_d = eᴬᵀ - // B_d = A⁻¹(eᴬᵀ - I)B - // A = −kᵥ/kₐ - // B = 1/kₐ double A = -kv / ka; double B = 1.0 / ka; double A_d = Math.exp(A * m_dt); double B_d = 1.0 / A * (A_d - 1.0) * B; - return Volts.of( + output.mut_replace( ks * Math.signum(currentVelocity.magnitude()) - + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude())); + + 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()), + Volts); + return output; } } @@ -242,9 +211,11 @@ public Voltage calculate( * simultaneously achievable - enter the acceleration constraint, and this will give you a * simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the motor. - * @param acceleration The acceleration of the motor. - * @return The maximum possible velocity at the given acceleration. + *

The units should be radians for angular systems and meters for linear systems. + * + * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts. + * @param acceleration The acceleration of the motor, in (units/s²). + * @return The maximum possible velocity in (units/s) at the given acceleration. */ public double maxAchievableVelocity(double maxVoltage, double acceleration) { // Assume max velocity is positive @@ -257,9 +228,11 @@ public double maxAchievableVelocity(double maxVoltage, double acceleration) { * simultaneously achievable - enter the acceleration constraint, and this will give you a * simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the motor. - * @param acceleration The acceleration of the motor. - * @return The minimum possible velocity at the given acceleration. + *

The units should be radians for angular systems and meters for linear systems. + * + * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts. + * @param acceleration The acceleration of the motor, in (units/s²). + * @return The maximum possible velocity in (units/s) at the given acceleration. */ public double minAchievableVelocity(double maxVoltage, double acceleration) { // Assume min velocity is negative, ks flips sign @@ -272,9 +245,11 @@ public double minAchievableVelocity(double maxVoltage, double acceleration) { * simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the motor. - * @param velocity The velocity of the motor. - * @return The maximum possible acceleration at the given velocity. + *

The units should be radians for angular systems and meters for linear systems. + * + * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts. + * @param velocity The velocity of the motor, in (units/s). + * @return The maximum possible acceleration in (units/s²) at the given velocity. */ public double maxAchievableAcceleration(double maxVoltage, double velocity) { return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka; @@ -286,9 +261,11 @@ public double maxAchievableAcceleration(double maxVoltage, double velocity) { * simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the motor. - * @param velocity The velocity of the motor. - * @return The minimum possible acceleration at the given velocity. + *

The units should be radians for angular systems and meters for linear systems. + * + * @param maxVoltage The maximum voltage that can be supplied to the motor, in volts. + * @param velocity The velocity of the motor, in (units/s). + * @return The maximum possible acceleration in (units/s²) at the given velocity. */ public double minAchievableAcceleration(double maxVoltage, double velocity) { return maxAchievableAcceleration(-maxVoltage, velocity); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java index fe3ce5757e8..eb4e5208052 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java @@ -27,7 +27,7 @@ public ProtobufArmFeedforward createMessage() { @Override public ArmFeedforward unpack(ProtobufArmFeedforward msg) { - return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa()); + return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt()); } @Override @@ -36,5 +36,6 @@ public void pack(ProtobufArmFeedforward msg, ArmFeedforward value) { msg.setKg(value.getKg()); msg.setKv(value.getKv()); msg.setKa(value.getKa()); + msg.setDt(value.getDt()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java index 5ccf70f669c..dbaafef6b49 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java @@ -28,7 +28,7 @@ public ProtobufElevatorFeedforward createMessage() { @Override public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) { - return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa()); + return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt()); } @Override @@ -37,5 +37,6 @@ public void pack(ProtobufElevatorFeedforward msg, ElevatorFeedforward value) { msg.setKg(value.getKg()); msg.setKv(value.getKv()); msg.setKa(value.getKa()); + msg.setDt(value.getDt()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java index b0d1b832cb4..2bb80de1535 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java @@ -21,12 +21,12 @@ public String getTypeName() { @Override public int getSize() { - return kSizeDouble * 4; + return kSizeDouble * 5; } @Override public String getSchema() { - return "double ks;double kg;double kv;double ka"; + return "double ks;double kg;double kv;double ka;double dt"; } @Override @@ -35,7 +35,8 @@ public ArmFeedforward unpack(ByteBuffer bb) { double kg = bb.getDouble(); double kv = bb.getDouble(); double ka = bb.getDouble(); - return new ArmFeedforward(ks, kg, kv, ka); + double dt = bb.getDouble(); + return new ArmFeedforward(ks, kg, kv, ka, dt); } @Override @@ -44,5 +45,6 @@ public void pack(ByteBuffer bb, ArmFeedforward value) { bb.putDouble(value.getKg()); bb.putDouble(value.getKv()); bb.putDouble(value.getKa()); + bb.putDouble(value.getDt()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java index f43eb6b69cd..1a0e8b77e65 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java @@ -21,12 +21,12 @@ public String getTypeName() { @Override public int getSize() { - return kSizeDouble * 4; + return kSizeDouble * 5; } @Override public String getSchema() { - return "double ks;double kg;double kv;double ka"; + return "double ks;double kg;double kv;double ka;double dt"; } @Override @@ -35,7 +35,8 @@ public ElevatorFeedforward unpack(ByteBuffer bb) { double kg = bb.getDouble(); double kv = bb.getDouble(); double ka = bb.getDouble(); - return new ElevatorFeedforward(ks, kg, kv, ka); + double dt = bb.getDouble(); + return new ElevatorFeedforward(ks, kg, kv, ka, dt); } @Override @@ -44,5 +45,6 @@ public void pack(ByteBuffer bb, ElevatorFeedforward value) { bb.putDouble(value.getKg()); bb.putDouble(value.getKv()); bb.putDouble(value.getKa()); + bb.putDouble(value.getDt()); } } diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp index 4cfaf89157b..e9848593a49 100644 --- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -19,6 +19,19 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t currentAngle, units::unit_t currentVelocity, units::unit_t nextVelocity, units::second_t dt) const { + return Calculate(currentAngle, currentVelocity, nextVelocity); +} + +units::volt_t ArmFeedforward::Calculate( + units::unit_t currentAngle, + units::unit_t currentVelocity) const { + return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) + + kV * currentVelocity; +} + +units::volt_t ArmFeedforward::Calculate( + units::unit_t currentAngle, units::unit_t currentVelocity, + units::unit_t nextVelocity) const { using VarMat = sleipnir::VariableMatrix; // Arm dynamics @@ -36,12 +49,12 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t currentAngle, sleipnir::Variable u_k; // Initial guess - auto acceleration = (nextVelocity - currentVelocity) / dt; + auto acceleration = (nextVelocity - currentVelocity) / m_dt; u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity + kA * acceleration + kG * units::math::cos(currentAngle)) .value()); - auto r_k1 = RK4(f, r_k, u_k, dt); + auto r_k1 = RK4(f, r_k, u_k, m_dt); // Minimize difference between desired and actual next velocity auto cost = diff --git a/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp b/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp index ca3a072d644..54748f151f8 100644 --- a/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp @@ -26,10 +26,11 @@ Java_edu_wpi_first_math_jni_ArmFeedforwardJNI_calculate { return frc::ArmFeedforward{units::volt_t{ks}, units::volt_t{kg}, units::unit_t{kv}, - units::unit_t{ka}} + units::unit_t{ka}, + units::second_t{dt}} .Calculate(units::radian_t{currentAngle}, units::radians_per_second_t{currentVelocity}, - units::radians_per_second_t{nextVelocity}, units::second_t{dt}) + units::radians_per_second_t{nextVelocity}) .value(); } diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index f5647324ac7..9503b5ecb43 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -37,11 +37,16 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param kG The gravity gain, in volts. * @param kV The velocity gain, in volt seconds per radian. * @param kA The acceleration gain, in volt seconds² per radian. + * @param dt The period in seconds. + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. + * @throws IllegalArgumentException for period ≤ zero. */ constexpr ArmFeedforward( units::volt_t kS, units::volt_t kG, units::unit_t kV, - units::unit_t kA = units::unit_t(0)) - : kS(kS), kG(kG), kV(kV), kA(kA) { + units::unit_t kA = units::unit_t(0), + units::second_t dt = 20_ms) + : kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) { if (kV.value() < 0) { wpi::math::MathSharedStore::ReportError( "kV must be a non-negative number, got {}!", kV.value()); @@ -54,45 +59,84 @@ class WPILIB_DLLEXPORT ArmFeedforward { this->kA = units::unit_t{0}; wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;"); } + if (dt <= 0_ms) { + wpi::math::MathSharedStore::ReportError( + "period must be a positive number, got {}!", dt.value()); + this->m_dt = 20_ms; + wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms."); + } } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous + * control. * * @param angle The angle setpoint, in radians. This angle should be * measured from the horizontal (i.e. if the provided * angle is 0, the arm should be parallel to the floor). * If your encoder does not follow this convention, an * offset should be added. - * @param velocity The velocity setpoint, in radians per second. - * @param acceleration The acceleration setpoint, in radians per second². + * @param velocity The velocity setpoint. + * @param acceleration The acceleration setpoint. * @return The computed feedforward, in volts. */ + [[deprecated("Use the current/next velocity overload instead.")]] units::volt_t Calculate(units::unit_t angle, units::unit_t velocity, - units::unit_t acceleration = - units::unit_t(0)) const { + units::unit_t acceleration) const { return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) + kV * velocity + kA * acceleration; } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous + * control. * * @param currentAngle The current angle in radians. This angle should be * measured from the horizontal (i.e. if the provided angle is 0, the arm * should be parallel to the floor). If your encoder does not follow this * convention, an offset should be added. - * @param currentVelocity The current velocity setpoint in radians per second. - * @param nextVelocity The next velocity setpoint in radians per second. + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. * @param dt Time between velocity setpoints in seconds. * @return The computed feedforward in volts. */ + [[deprecated("Use the current/next velocity overload instead.")]] units::volt_t Calculate(units::unit_t currentAngle, units::unit_t currentVelocity, units::unit_t nextVelocity, units::second_t dt) const; + /** + * Calculates the feedforward from the gains and setpoint assuming discrete + * control. Use this method when the velocity does not change. + * + * @param currentAngle The current angle. This angle should be measured from + * the horizontal (i.e. if the provided angle is 0, the arm should be parallel + * to the floor). If your encoder does not follow this convention, an offset + * should be added. + * @param currentVelocity The current velocity. + * @return The computed feedforward in volts. + */ + units::volt_t Calculate(units::unit_t currentAngle, + units::unit_t currentVelocity) const; + + /** + * Calculates the feedforward from the gains and setpoints assuming discrete + * control. + * + * @param currentAngle The current angle. This angle should be measured from + * the horizontal (i.e. if the provided angle is 0, the arm should be parallel + * to the floor). If your encoder does not follow this convention, an offset + * should be added. + * @param currentVelocity The current velocity. + * @param nextVelocity The next velocity. + * @return The computed feedforward in volts. + */ + units::volt_t Calculate(units::unit_t currentAngle, + units::unit_t currentVelocity, + units::unit_t nextVelocity) const; + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: @@ -232,6 +276,9 @@ class WPILIB_DLLEXPORT ArmFeedforward { /// The acceleration gain, in V/(rad/s²). units::unit_t kA; + + /** The period. */ + units::second_t m_dt; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index 6c7f9279797..d3f77066016 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -37,11 +37,16 @@ class ElevatorFeedforward { * @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² per distance. + * @param dt The period in seconds. + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. + * @throws IllegalArgumentException for period ≤ zero. */ constexpr ElevatorFeedforward( units::volt_t kS, units::volt_t kG, units::unit_t kV, - units::unit_t kA = units::unit_t(0)) - : kS(kS), kG(kG), kV(kV), kA(kA) { + units::unit_t kA = units::unit_t(0), + units::second_t dt = 20_ms) + : kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) { if (kV.value() < 0) { wpi::math::MathSharedStore::ReportError( "kV must be a non-negative number, got {}!", kV.value()); @@ -54,55 +59,43 @@ class ElevatorFeedforward { this->kA = units::unit_t{0}; wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;"); } + if (dt <= 0_ms) { + wpi::math::MathSharedStore::ReportError( + "period must be a positive number, got {}!", dt.value()); + this->m_dt = 20_ms; + wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms."); + } } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous + * control. * - * @param velocity The velocity setpoint, in distance per second. - * @param acceleration The acceleration setpoint, in distance per second². + * @param velocity The velocity setpoint. + * @param acceleration The acceleration setpoint. * @return The computed feedforward, in volts. + * @deprecated Use the current/next velocity overload instead. */ - constexpr units::volt_t Calculate(units::unit_t velocity, - units::unit_t acceleration = - units::unit_t(0)) { + [[deprecated("Use the current/next velocity overload instead.")]] + units::volt_t Calculate(units::unit_t velocity, + units::unit_t acceleration) const { return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration; } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous + * control. * - * @param currentVelocity The current velocity setpoint, in distance per - * second. - * @param nextVelocity The next velocity setpoint, in distance per second. + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. * @param dt Time between velocity setpoints in seconds. * @return The computed feedforward, in volts. */ + [[deprecated("Use the current/next velocity overload instead.")]] units::volt_t Calculate(units::unit_t currentVelocity, units::unit_t nextVelocity, units::second_t dt) const { - // Discretize the affine model. - // - // dx/dt = Ax + Bu + c - // dx/dt = Ax + B(u + B⁺c) - // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ - // - // Solve for uₖ. - // - // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ - // - // For an elevator with the model - // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), - // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B - // assuming sgn(x) is a constant for the duration of the step. - // - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) + // See wpimath/algorithms.md#Elevator_feedforward for derivation auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; @@ -113,6 +106,46 @@ class ElevatorFeedforward { units::volt_t{feedforward.Calculate(r, nextR)(0)}; } + /** + * Calculates the feedforward from the gains and setpoint assuming discrete + * control. Use this method when the setpoint does not change. + * + * @param currentVelocity The velocity setpoint. + * @return The computed feedforward, in volts. + */ + constexpr units::volt_t Calculate( + units::unit_t currentVelocity) const { + return Calculate(currentVelocity, currentVelocity); + } + + /** + * Calculates the feedforward from the gains and setpoints assuming discrete + * control. + * + *

Note this method is inaccurate when the velocity crosses 0. + * + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. + * @return The computed feedforward, in volts. + */ + constexpr units::volt_t Calculate( + units::unit_t currentVelocity, + units::unit_t nextVelocity) const { + // See wpimath/algorithms.md#Elevator_feedforward for derivation + if (kA == decltype(kA)(0)) { + return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity; + } else { + double A = -kV.value() / kA.value(); + double B = 1.0 / kA.value(); + double A_d = gcem::exp(A * m_dt.value()); + double B_d = 1.0 / A * (A_d - 1.0) * B; + return kG + kS * wpi::sgn(currentVelocity) + + units::volt_t{ + 1.0 / B_d * + (nextVelocity.value() - A_d * currentVelocity.value())}; + } + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: @@ -222,6 +255,9 @@ class ElevatorFeedforward { /// The acceleration gain. units::unit_t kA; + + /** The period. */ + units::second_t m_dt; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 5c400501e4e..94f9134fdbd 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -7,6 +7,8 @@ #include #include +#include "units/angle.h" +#include "units/length.h" #include "units/time.h" #include "units/voltage.h" #include "wpimath/MathShared.h" @@ -18,6 +20,8 @@ namespace frc { * permanent-magnet DC motor. */ template + requires std::same_as || + std::same_as class SimpleMotorFeedforward { public: using Velocity = @@ -35,6 +39,9 @@ class SimpleMotorFeedforward { * @param kV The velocity gain, in volt seconds per distance. * @param kA The acceleration gain, in volt seconds² per distance. * @param dt The period in seconds. + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. + * @throws IllegalArgumentException for period ≤ zero. */ constexpr SimpleMotorFeedforward( units::volt_t kS, units::unit_t kV, @@ -62,10 +69,11 @@ class SimpleMotorFeedforward { } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming continuous + * control. * - * @param velocity The velocity setpoint, in distance per second. - * @param acceleration The acceleration setpoint, in distance per second². + * @param velocity The velocity setpoint. + * @param acceleration The acceleration setpoint. * @return The computed feedforward, in volts. * @deprecated Use the current/next velocity overload instead. */ @@ -77,11 +85,10 @@ class SimpleMotorFeedforward { } /** - * Calculates the feedforward from the gains and setpoint. - * Use this method when the setpoint does not change. + * Calculates the feedforward from the gains and setpoint assuming discrete + * control. Use this method when the setpoint does not change. * - * @param setpoint The velocity setpoint, in distance per - * second. + * @param setpoint The velocity setpoint. * @return The computed feedforward, in volts. */ constexpr units::volt_t Calculate(units::unit_t setpoint) const { @@ -89,70 +96,22 @@ class SimpleMotorFeedforward { } /** - * Calculates the feedforward from the gains and setpoints. + * Calculates the feedforward from the gains and setpoints assuming discrete + * control. * - * @param currentVelocity The current velocity setpoint, in distance per - * second. - * @param nextVelocity The next velocity setpoint, in distance per second. + *

Note this method is inaccurate when the velocity crosses 0. + * + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. * @return The computed feedforward, in volts. */ constexpr units::volt_t Calculate( units::unit_t currentVelocity, units::unit_t nextVelocity) const { + // See wpimath/algorithms.md#Simple_motor_feedforward for derivation if (kA == decltype(kA)(0)) { - // Given the following discrete feedforward model - // - // uₖ = B_d⁺(rₖ₊₁ − A_d rₖ) - // - // where - // - // A_d = eᴬᵀ - // B_d = A⁻¹(eᴬᵀ - I)B - // A = −kᵥ/kₐ - // B = 1/kₐ - // - // We want the feedforward model when kₐ = 0. - // - // Simplify A. - // - // A = −kᵥ/kₐ - // - // As kₐ approaches zero, A approaches -∞. - // - // A = −∞ - // - // Simplify A_d. - // - // A_d = eᴬᵀ - // A_d = std::exp(−∞) - // A_d = 0 - // - // Simplify B_d. - // - // B_d = A⁻¹(eᴬᵀ - I)B - // B_d = A⁻¹((0) - I)B - // B_d = A⁻¹(-I)B - // B_d = -A⁻¹B - // B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ) - // B_d = (kᵥ/kₐ)⁻¹(1/kₐ) - // B_d = kₐ/kᵥ(1/kₐ) - // B_d = 1/kᵥ - // - // Substitute these into the feedforward equation. - // - // uₖ = B_d⁺(rₖ₊₁ − A_d rₖ) - // uₖ = (1/kᵥ)⁺(rₖ₊₁ − (0) rₖ) - // uₖ = kᵥrₖ₊₁ return kS * wpi::sgn(nextVelocity) + kV * nextVelocity; } else { - // uₖ = B_d⁺(rₖ₊₁ − A_d rₖ) - // - // where - // - // A_d = eᴬᵀ - // B_d = A⁻¹(eᴬᵀ - I)B - // A = −kᵥ/kₐ - // B = 1/kₐ double A = -kV.value() / kA.value(); double B = 1.0 / kA.value(); double A_d = gcem::exp(A * m_dt.value()); diff --git a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h index bcd8ffc936d..1ebbefa3633 100644 --- a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h +++ b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h @@ -10,7 +10,8 @@ #include "units/length.h" // Everything is converted into units for -// frc::SimpleMotorFeedforward +// frc::SimpleMotorFeedforward or +// frc::SimpleMotorFeedforward template struct wpi::Struct> { @@ -31,6 +32,6 @@ struct wpi::Struct> { static_assert( wpi::StructSerializable>); static_assert( - wpi::StructSerializable>); + wpi::StructSerializable>); #include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc" diff --git a/wpimath/src/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto index dd9db5f3110..c5e53ea09f3 100644 --- a/wpimath/src/main/proto/controller.proto +++ b/wpimath/src/main/proto/controller.proto @@ -9,6 +9,7 @@ message ProtobufArmFeedforward { double kg = 2; double kv = 3; double ka = 4; + double dt = 5; } message ProtobufDifferentialDriveFeedforward { @@ -23,6 +24,7 @@ message ProtobufElevatorFeedforward { double kg = 2; double kv = 3; double ka = 4; + double dt = 5; } message ProtobufSimpleMotorFeedforward { diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java index d33af05d33f..ecfddcf6bec 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java @@ -4,6 +4,9 @@ package edu.wpi.first.math.controller; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -68,19 +71,23 @@ private Matrix simulate( */ private void calculateAndSimulate( double currentAngle, double currentVelocity, double nextVelocity, double dt) { - final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity, dt); + final double input = + m_armFF + .calculate( + Radians.of(currentAngle), + RadiansPerSecond.of(currentVelocity), + RadiansPerSecond.of(nextVelocity)) + .in(Volts); assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12); } @Test void testCalculate() { // calculate(angle, angular velocity) - assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002); - assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002); - - // calculate(angle, angular velocity, angular acceleration) - assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002); - assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002); + assertEquals( + 0.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(0)).in(Volts), 0.002); + assertEquals( + 2.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(1)).in(Volts), 0.002); // calculate(currentAngle, currentVelocity, nextAngle, dt) calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index a963d507bde..c4161a0b979 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.controller; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; @@ -24,10 +26,8 @@ class ElevatorFeedforwardTest { @Test void testCalculate() { - assertEquals(1, m_elevatorFF.calculate(0), 0.002); - assertEquals(4.5, m_elevatorFF.calculate(2), 0.002); - assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); - assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002); + assertEquals(1, m_elevatorFF.calculate(MetersPerSecond.of(0)).in(Volts), 0.002); + assertEquals(4.5, m_elevatorFF.calculate(MetersPerSecond.of(2)).in(Volts), 0.002); var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka); var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka); @@ -38,7 +38,7 @@ void testCalculate() { var nextR = VecBuilder.fill(3.0); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, - m_elevatorFF.calculate(2.0, 3.0, dt), + m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts), 0.002); } diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp index 890c462cc1d..66060c20201 100644 --- a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp @@ -62,7 +62,7 @@ void CalculateAndSimulate(const frc::ArmFeedforward& armFF, units::radians_per_second_t currentVelocity, units::radians_per_second_t nextVelocity, units::second_t dt) { - auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity, dt); + auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity); EXPECT_NEAR(nextVelocity.value(), Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12); } @@ -80,17 +80,6 @@ TEST(ArmFeedforwardTest, Calculate) { armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s).value(), 2.5, 0.002); - // Calculate(angle, angular velocity, angular acceleration) - EXPECT_NEAR( - armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 2_rad_per_s_sq) - .value(), - 6.5, 0.002); - EXPECT_NEAR( - armFF - .Calculate(std::numbers::pi / 3 * 1_rad, -1_rad_per_s, 2_rad_per_s_sq) - .value(), - 2.5, 0.002); - // Calculate(currentAngle, currentVelocity, nextAngle, dt) CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 1.05_rad_per_s, 20_ms); diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index c6af43a1e73..e4048c0d0b3 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -23,10 +23,6 @@ TEST(ElevatorFeedforwardTest, Calculate) { EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002); - EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5, - 0.002); - EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5, - 0.002); frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()}; frc::Matrixd<1, 1> B{1.0 / Ka.value()}; @@ -36,7 +32,7 @@ TEST(ElevatorFeedforwardTest, Calculate) { frc::Vectord<1> r{2.0}; frc::Vectord<1> nextR{3.0}; EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), - elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002); + elevatorFF.Calculate(2_mps, 3_mps).value(), 0.002); } TEST(ElevatorFeedforwardTest, AchievableVelocity) {