The CTRE Phoenix 6 Swerve contains a FieldCentricFacingAngle request, which allows you to add a target rate feedforward for the rotation of the drivetrain. The generated code also contains a SysId routine to determine feedforward values as well as P and D values.
So, that’s great and all, but uh… how do I actually implement these feedforward values? I understand the WPILib SimpleFeedForward classes, but how do I get the Setpoint/Next Velocity in order to calculate it?
I’m a little lost on how to approach this. Any help is appreciated!
I’m a little confused. Where is this “setTarget” method you are referring to? I’m looking at the Phoenix 6 documentation and can’t find any reference to that.
// Note that X is defined as forward according to WPILib convention,
// and Y is defined as to the left according to WPILib convention.
drivetrain.setDefaultCommand(
// Drivetrain will execute this comm
[details="Summary"]
This text will be hidden
[/details]
and periodically
drivetrain.applyRequest(() ->
drive.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
.withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left)
)
);
I’ll try to explain this with an example of how my team used this. This year we used fieldcentricfacingangle to make sure we were able to follow a path in auto. The path generator we used created a file that gave the target angle and angular velocity at each point in time.
To start we just inputted the x velocity, y velocity and angle into the request. What happened then was that the robot would only start spinning after some time not when it was supposed to. The reason for this is that PID only responds to error. At the beginning of the path the error was zero and until the setpoint had changed enough the PID was too small to make a difference. This caused a problem where the setpoint would have to changr to something like 15 degrees before we would start rotating.
So how did we fix this? The fieldcentricfacingangle request has an additional parameter of the rotational velocity you want. This is the feedforward term. By adding the the angular velocity generated by the path as feedforward then we did not have to get to such high errors to begin rotating and the PID only corrected small errors. The feedforward did most of the work and the PID only corrected small errors that occurred.
The point of the feedforward is to prevent the errors before they happen. In the case of following a path that means adding the wanted angular velocity from the path. In the case of driving with a joystick there are ways to do this but they are more complicated. If you want I can explain them.
For context, my team is planning to use FieldCentricFacingAngle to allow us to rotate to a fixed angle quickly without having the driver needing to spin the robot manually. We want to implement a feedforward in order for the robot to quickly reach the angle regardless of how far away we are from the target.
public class FieldCentricFacingAngle implements SwerveRequest {
/**
* The velocity in the X direction, in m/s.
* X is defined as forward according to WPILib convention,
* so this determines how fast to travel forward.
*/
public double VelocityX = 0;
/**
* The velocity in the Y direction, in m/s.
* Y is defined as to the left according to WPILib convention,
* so this determines how fast to travel to the left.
*/
public double VelocityY = 0;
/**
* The desired direction to face.
* 0 Degrees is defined as in the direction of the X axis.
* As a result, a TargetDirection of 90 degrees will point along
* the Y axis, or to the left.
*/
public Rotation2d TargetDirection = new Rotation2d();
/**
* The allowable deadband of the request.
*/
public double Deadband = 0;
/**
* The rotational deadband of the request.
*/
public double RotationalDeadband = 0;
/**
* The center of rotation the robot should rotate around.
* This is (0,0) by default, which will rotate around the center of the robot.
*/
public Translation2d CenterOfRotation = new Translation2d();
/**
* The type of control request to use for the drive motor.
*/
public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
/**
* The type of control request to use for the steer motor.
*/
public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.MotionMagic;
/**
* The PID controller used to maintain the desired heading.
* Users can specify the PID gains to change how aggressively to maintain
* heading.
* <p>
* This PID controller operates on heading radians and outputs a target
* rotational rate in radians per second.
*/
public PhoenixPIDController HeadingController = new PhoenixPIDController(0, 0, 0);
/**
* The perspective to use when determining which direction is forward.
*/
public ForwardReference ForwardReference = SwerveRequest.ForwardReference.OperatorPerspective;
public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) {
double toApplyX = VelocityX;
double toApplyY = VelocityY;
Rotation2d angleToFace = TargetDirection;
if (ForwardReference == SwerveRequest.ForwardReference.OperatorPerspective) {
/* If we're operator perspective, modify the X/Y translation by the angle */
Translation2d tmp = new Translation2d(toApplyX, toApplyY);
tmp = tmp.rotateBy(parameters.operatorForwardDirection);
toApplyX = tmp.getX();
toApplyY = tmp.getY();
/* And rotate the direction we want to face by the angle */
angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection);
}
double rotationRate = HeadingController.calculate(parameters.currentPose.getRotation().getRadians(),
angleToFace.getRadians(), parameters.timestamp);
double toApplyOmega = rotationRate;
if (Math.sqrt(toApplyX * toApplyX + toApplyY * toApplyY) < Deadband) {
toApplyX = 0;
toApplyY = 0;
}
if (Math.abs(toApplyOmega) < RotationalDeadband) {
toApplyOmega = 0;
}
ChassisSpeeds speeds = ChassisSpeeds.discretize(ChassisSpeeds.fromFieldRelativeSpeeds(toApplyX, toApplyY, toApplyOmega,
parameters.currentPose.getRotation()), parameters.updatePeriod);
var states = parameters.kinematics.toSwerveModuleStates(speeds, CenterOfRotation);
for (int i = 0; i < modulesToApply.length; ++i) {
modulesToApply[i].apply(states[i], DriveRequestType, SteerRequestType);
}
return StatusCode.OK;
}
/**
* Sets the velocity in the X direction, in m/s.
* X is defined as forward according to WPILib convention,
* so this determines how fast to travel forward.
*
* @param velocityX Velocity in the X direction, in m/s
* @return this request
*/
public FieldCentricFacingAngle withVelocityX(double velocityX) {
this.VelocityX = velocityX;
return this;
}
/**
* Sets the velocity in the Y direction, in m/s.
* Y is defined as to the left according to WPILib convention,
* so this determines how fast to travel to the left.
*
* @param velocityY Velocity in the Y direction, in m/s
* @return this request
*/
public FieldCentricFacingAngle withVelocityY(double velocityY) {
this.VelocityY = velocityY;
return this;
}
/**
* Sets the desired direction to face.
* 0 Degrees is defined as in the direction of the X axis.
* As a result, a TargetDirection of 90 degrees will point along
* the Y axis, or to the left.
*
* @param targetDirection Desired direction to face
* @return this request
*/
public FieldCentricFacingAngle withTargetDirection(Rotation2d targetDirection) {
this.TargetDirection = targetDirection;
return this;
}
/**
* Sets the allowable deadband of the request.
*
* @param deadband Allowable deadband of the request
* @return this request
*/
public FieldCentricFacingAngle withDeadband(double deadband) {
this.Deadband = deadband;
return this;
}
/**
* Sets the rotational deadband of the request.
*
* @param rotationalDeadband Rotational deadband of the request
* @return this request
*/
public FieldCentricFacingAngle withRotationalDeadband(double rotationalDeadband) {
this.RotationalDeadband = rotationalDeadband;
return this;
}
/**
* Sets the center of rotation of the request
*
* @param centerOfRotation The center of rotation the robot should rotate around.
* @return this request
*/
public FieldCentricFacingAngle withCenterOfRotation(Translation2d centerOfRotation) {
this.CenterOfRotation = centerOfRotation;
return this;
}
/**
* Sets the type of control request to use for the drive motor.
*
* @param driveRequestType The type of control request to use for the drive motor
* @return this request
*/
public FieldCentricFacingAngle withDriveRequestType(SwerveModule.DriveRequestType driveRequestType) {
this.DriveRequestType = driveRequestType;
return this;
}
/**
* Sets the type of control request to use for the steer motor.
*
* @param steerRequestType The type of control request to use for the steer motor
* @return this request
*/
public FieldCentricFacingAngle withSteerRequestType(SwerveModule.SteerRequestType steerRequestType) {
this.SteerRequestType = steerRequestType;
return this;
}
}
Looks like I made a mistake. The feedforward part is only in the new betas. I guess we did not use FieldCentricFacingAngle in our code but we used the same algorithm.
Going to bump this, since my question has not been answered yet. CTRE provides these routines, but I have yet to find an example of these feedforwards being used in any robot project or provided example.
Has anyone been able to implement the rotation feedforwards with FieldCentricFacingAngle requests?
private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(100).withKI(0).withKD(2.0)
.withKS(0.2).withKV(1.59).withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains = new Slot0Configs()
.withKP(0.1).withKI(0).withKD(0)
.withKS(0).withKV(0.124);
this is to set the FF and this is to add it to the FieldCentricFacingAngle
private final SwerveRequest.FieldCentricFacingAngle pointFacingAngle = new SwerveRequest.FieldCentricFacingAngle()
.withDriveRequestType(DriveRequestType.OpenLoopVoltage).withTargetRateFeedforward(5);
ctre TargetRateFeedforward - is only to control the speed of it and i’m pretty sure the values are good as is. the only thing you need to do is run SYSID and get the KV kS. and math will do the rest.
If you are controlling the target direction with something like a joystick or vision, then you would only use the TargetRateFeedforward if you apply a motion profile to the target direction. We do not apply any motion profile OOTB in our FieldCentricFacingAngle request. If you want to apply a motion profile, then the TargetRateFeedforward would just be set to the velocity of the current profile setpoint.
Attached below is an example custom swerve request that adds a TrapezoidProfile to the TargetDirection and uses our FieldCentricFacingAngle under the hood.
Instead of creating a custom swerve request, you could put the motion profile logic in CommandSwerveDrivetrain and directly use FieldCentricFacingAngle. Note that you would need to cache the operator forward direction since we currently don’t provide a getter for it, and the profile would only update at 50 Hz instead of the full 250 Hz of the custom swerve request, which should be fine.
Usage of the custom swerve request is then the same as the regular FieldCentricFacingAngle request:
private final ProfiledFieldCentricFacingAngle driveFacingAngle =
new ProfiledFieldCentricFacingAngle(new TrapezoidProfile.Constraints(MaxAngularRate, MaxAngularRate / 0.25))
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
private void configureBindings() {
drive.HeadingController.setPID(7, 0, 0);
// Note that X is defined as forward according to WPILib convention,
// and Y is defined as to the left according to WPILib convention.
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
drivetrain.runOnce(() -> driveFacingAngle.resetProfile(drivetrain.getState().Pose.getRotation()))
.andThen(
drivetrain.applyRequest(() ->
driveFacingAngle.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
.withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
.withTargetDirection(new Rotation2d(-joystick.getRightY(), -joystick.getRightX())) // Drive to target angle using right joystick
)
)
);
// ...
}
This isn’t quite right, the TargetRateFeedforward is a value in radians/second that is directly added to the target rotational speed of the drivetrain. So setting it to a constant value means the drivetrain will try to rotate at a constant velocity, which will go against the PID that is trying to hold a constant heading.