Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.differentialdrivebot;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -79,8 +82,10 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);
final double rightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.ComputerVisionUtil;
@@ -139,8 +142,10 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);
final double rightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -75,11 +78,11 @@ public class DriveSubsystem extends SubsystemBase {
m_leftLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
left.position,
m_feedforward.calculate(left.velocity));
m_feedforward.calculate(MetersPerSecond.of(left.velocity)).in(Volts));
m_rightLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
right.position,
m_feedforward.calculate(right.velocity));
m_feedforward.calculate(MetersPerSecond.of(right.velocity)).in(Volts));
}
/**

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.ExponentialProfile;
import edu.wpi.first.wpilibj.Joystick;
@@ -46,7 +49,7 @@ public class Robot extends TimedRobot {
m_motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.02) / 12.0);
m_feedforward.calculate(RadiansPerSecond.of(next.velocity)).in(Volts) / 12.0);
m_setpoint = next;
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Joystick;
@@ -46,6 +49,6 @@ public class Robot extends TimedRobot {
m_motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity) / 12.0);
m_feedforward.calculate(MetersPerSecond.of(m_setpoint.velocity)).in(Volts) / 12.0);
}
}

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.eventloop;
import static edu.wpi.first.units.Units.RPM;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
@@ -61,10 +64,12 @@ public class Robot extends TimedRobot {
shootTrigger
// accelerate the shooter wheel
.ifHigh(
() ->
m_shooter.setVoltage(
m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY)
+ m_ff.calculate(SHOT_VELOCITY)));
() -> {
m_shooter.setVoltage(
m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY)
+ m_ff.calculate(RPM.of(SHOT_VELOCITY)).in(Volts));
});
// if not, stop
shootTrigger.negate().ifHigh(m_shooter::stopMotor);

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.numbers.N1;
@@ -87,7 +90,8 @@ public class Robot extends TimedRobot {
// Controls a motor with the output of the BangBang controller and a
// feedforward. The feedforward is reduced slightly to avoid overspeeding
// the shooter.
m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint));
m_flywheelMotor.setVoltage(
bangOutput + 0.9 * m_feedforward.calculate(RadiansPerSecond.of(setpoint)).in(Volts));
}
/** Update our simulation. This should be run every robot loop in simulation. */

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
@@ -33,7 +36,11 @@ public class ShooterSubsystem extends PIDSubsystem {
@Override
public void useOutput(double output, double setpoint) {
m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
m_shooterMotor.setVoltage(
output
+ m_shooterFeedforward
.calculate(RadiansPerSecond.of(ShooterConstants.kShooterTargetRPS))
.in(Volts));
}
@Override

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.mecanumbot;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
@@ -95,10 +98,14 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
final double frontLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts);
final double frontRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts);
final double backLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts);
final double backRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -107,10 +110,14 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
final double frontLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts);
final double frontRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts);
final double backLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts);
final double backRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(

16
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.wpilibj2.command.Commands.parallel;
import static edu.wpi.first.wpilibj2.command.Commands.waitUntil;
@@ -54,11 +56,15 @@ public class Shooter extends SubsystemBase {
return parallel(
// Run the shooter flywheel at the desired setpoint using feedforward and feedback
run(
() ->
m_shooterMotor.set(
m_shooterFeedforward.calculate(setpointRotationsPerSecond)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpointRotationsPerSecond))),
() -> {
m_shooterMotor.set(
m_shooterFeedforward
.calculate(RotationsPerSecond.of(setpointRotationsPerSecond))
.in(Volts)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpointRotationsPerSecond));
}),
// Wait until the shooter has reached the setpoint, and then run the feeder
waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.set(1)))
.withName("Shoot");

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
@@ -94,8 +97,10 @@ public class Drivetrain {
/** Sets speeds to the drivetrain motors. */
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);
final double rightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts);
double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
double rightOutput =

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.wpilibj.examples.swervebot;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -119,17 +123,21 @@ public class SwerveModule {
state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
final double driveFeedforward =
m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
m_turnFeedforward
.calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity))
.in(Volts);
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -122,14 +126,17 @@ public class SwerveModule {
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
final double driveFeedforward =
m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
m_turnFeedforward
.calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity))
.in(Volts);
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java
View File

@@ -98,7 +98,9 @@ public class Shooter extends SubsystemBase {
return run(() -> {
m_shooterMotor.setVoltage(
m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble())
+ m_shooterFeedforward.calculate(shooterSpeed.getAsDouble()));
+ m_shooterFeedforward
.calculate(RotationsPerSecond.of(shooterSpeed.getAsDouble()))
.in(Volts));
m_feederMotor.set(ShooterConstants.kFeederSpeed);
})
.finallyDo(