mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[command] Modify swerve and mecanum commands to use new controller
This commit is contained in:
Prateek Machiraju
committed by
Peter Johnson
Peter Johnson
parent
5ca2702083
commit
aba035eb3d
@@ -11,10 +11,12 @@ import java.util.function.Consumer;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
|
||||
@@ -45,18 +47,13 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
@SuppressWarnings({"PMD.TooManyFields", "MemberName"})
|
||||
public class MecanumControllerCommand extends CommandBase {
|
||||
private final Timer m_timer = new Timer();
|
||||
private MecanumDriveWheelSpeeds m_prevSpeeds;
|
||||
private double m_prevTime;
|
||||
private Pose2d m_finalPose;
|
||||
private final boolean m_usePID;
|
||||
|
||||
private final Trajectory m_trajectory;
|
||||
private final Supplier<Pose2d> m_pose;
|
||||
private final SimpleMotorFeedforward m_feedforward;
|
||||
private final MecanumDriveKinematics m_kinematics;
|
||||
private final PIDController m_xController;
|
||||
private final PIDController m_yController;
|
||||
private final ProfiledPIDController m_thetaController;
|
||||
private final HolonomicDriveController m_controller;
|
||||
private final Supplier<Rotation2d> m_desiredRotation;
|
||||
private final double m_maxWheelVelocityMetersPerSecond;
|
||||
private final PIDController m_frontLeftController;
|
||||
private final PIDController m_rearLeftController;
|
||||
@@ -65,6 +62,8 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
|
||||
private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
|
||||
private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
|
||||
private MecanumDriveWheelSpeeds m_prevSpeeds;
|
||||
private double m_prevTime;
|
||||
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow the provided
|
||||
@@ -74,8 +73,105 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
|
||||
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The rotation controller will calculate the rotation based on the final pose in the
|
||||
* trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one of
|
||||
* the odometry classes to provide this.
|
||||
* @param feedforward The feedforward to use for the drivetrain.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the robot should be facing. This
|
||||
* is sampled at each time step.
|
||||
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
|
||||
* @param frontLeftController The front left wheel velocity PID.
|
||||
* @param rearLeftController The rear left wheel velocity PID.
|
||||
* @param frontRightController The front right wheel velocity PID.
|
||||
* @param rearRightController The rear right wheel velocity PID.
|
||||
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
|
||||
* the current wheel speeds.
|
||||
* @param outputDriveVoltages A MecanumDriveMotorVoltages object containing
|
||||
* the output motor voltages.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
|
||||
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
|
||||
public MecanumControllerCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
SimpleMotorFeedforward feedforward,
|
||||
MecanumDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
Supplier<Rotation2d> desiredRotation,
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
PIDController frontLeftController,
|
||||
PIDController rearLeftController,
|
||||
PIDController frontRightController,
|
||||
PIDController rearRightController,
|
||||
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
|
||||
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory",
|
||||
"MecanumControllerCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
|
||||
m_feedforward = requireNonNullParam(feedforward, "feedforward",
|
||||
"MecanumControllerCommand");
|
||||
m_kinematics = requireNonNullParam(kinematics, "kinematics",
|
||||
"MecanumControllerCommand");
|
||||
|
||||
m_controller = new HolonomicDriveController(
|
||||
requireNonNullParam(xController,
|
||||
"xController", "SwerveControllerCommand"),
|
||||
requireNonNullParam(yController,
|
||||
"xController", "SwerveControllerCommand"),
|
||||
requireNonNullParam(thetaController,
|
||||
"thetaController", "SwerveControllerCommand")
|
||||
);
|
||||
|
||||
m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
|
||||
"MecanumControllerCommand");
|
||||
|
||||
m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
|
||||
"maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
|
||||
|
||||
m_frontLeftController = requireNonNullParam(frontLeftController,
|
||||
"frontLeftController", "MecanumControllerCommand");
|
||||
m_rearLeftController = requireNonNullParam(rearLeftController,
|
||||
"rearLeftController", "MecanumControllerCommand");
|
||||
m_frontRightController = requireNonNullParam(frontRightController,
|
||||
"frontRightController", "MecanumControllerCommand");
|
||||
m_rearRightController = requireNonNullParam(rearRightController,
|
||||
"rearRightController", "MecanumControllerCommand");
|
||||
|
||||
m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
|
||||
"currentWheelSpeeds", "MecanumControllerCommand");
|
||||
|
||||
m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
|
||||
"outputDriveVoltages", "MecanumControllerCommand");
|
||||
|
||||
m_outputWheelSpeeds = null;
|
||||
|
||||
m_usePID = true;
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow the provided
|
||||
* trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
|
||||
* 12 as a voltage output to the motor.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
|
||||
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one of
|
||||
@@ -102,58 +198,100 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
|
||||
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
|
||||
public MecanumControllerCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
SimpleMotorFeedforward feedforward,
|
||||
MecanumDriveKinematics kinematics,
|
||||
Supplier<Pose2d> pose,
|
||||
SimpleMotorFeedforward feedforward,
|
||||
MecanumDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
PIDController frontLeftController,
|
||||
PIDController rearLeftController,
|
||||
PIDController frontRightController,
|
||||
PIDController rearRightController,
|
||||
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
|
||||
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
|
||||
Subsystem... requirements) {
|
||||
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
this(trajectory, pose, feedforward, kinematics, xController, yController, thetaController,
|
||||
() -> trajectory.getStates().get(trajectory.getStates().size() - 1)
|
||||
.poseMeters.getRotation(),
|
||||
maxWheelVelocityMetersPerSecond, frontLeftController, rearLeftController,
|
||||
frontRightController, rearRightController, currentWheelSpeeds, outputDriveVoltages,
|
||||
requirements);
|
||||
}
|
||||
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow the provided
|
||||
* trajectory. The user should implement a velocity PID on the desired output wheel velocities.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
|
||||
* this is left to the user, since it is not appropriate for paths with non-stationary end-states.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one of
|
||||
* the odometry classes to provide this.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the robot should be facing. This
|
||||
* is sampled at each time step.
|
||||
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
|
||||
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing
|
||||
* the output wheel speeds.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
|
||||
PIDController frontLeftController,
|
||||
PIDController rearLeftController,
|
||||
PIDController frontRightController,
|
||||
PIDController rearRightController,
|
||||
|
||||
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
|
||||
|
||||
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
|
||||
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
|
||||
public MecanumControllerCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
MecanumDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
Supplier<Rotation2d> desiredRotation,
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory",
|
||||
"MecanumControllerCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
|
||||
m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
|
||||
m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
|
||||
m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
|
||||
m_kinematics = requireNonNullParam(kinematics,
|
||||
"kinematics", "MecanumControllerCommand");
|
||||
|
||||
m_xController = requireNonNullParam(xController, "xController",
|
||||
"MecanumControllerCommand");
|
||||
m_yController = requireNonNullParam(yController, "yController",
|
||||
"MecanumControllerCommand");
|
||||
m_thetaController = requireNonNullParam(thetaController, "thetaController",
|
||||
"MecanumControllerCommand");
|
||||
m_controller = new HolonomicDriveController(
|
||||
requireNonNullParam(xController,
|
||||
"xController", "SwerveControllerCommand"),
|
||||
requireNonNullParam(yController,
|
||||
"xController", "SwerveControllerCommand"),
|
||||
requireNonNullParam(thetaController,
|
||||
"thetaController", "SwerveControllerCommand")
|
||||
);
|
||||
|
||||
m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
|
||||
"MecanumControllerCommand");
|
||||
|
||||
m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
|
||||
"maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
|
||||
"maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
|
||||
|
||||
m_frontLeftController = requireNonNullParam(frontLeftController,
|
||||
"frontLeftController", "MecanumControllerCommand");
|
||||
m_rearLeftController = requireNonNullParam(rearLeftController,
|
||||
"rearLeftController", "MecanumControllerCommand");
|
||||
m_frontRightController = requireNonNullParam(frontRightController,
|
||||
"frontRightController", "MecanumControllerCommand");
|
||||
m_rearRightController = requireNonNullParam(rearRightController,
|
||||
"rearRightController", "MecanumControllerCommand");
|
||||
m_frontLeftController = null;
|
||||
m_rearLeftController = null;
|
||||
m_frontRightController = null;
|
||||
m_rearRightController = null;
|
||||
|
||||
m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
|
||||
"currentWheelSpeeds", "MecanumControllerCommand");
|
||||
m_currentWheelSpeeds = null;
|
||||
|
||||
m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
|
||||
"outputDriveVoltages", "MecanumControllerCommand");
|
||||
m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
|
||||
"outputWheelSpeeds", "MecanumControllerCommand");
|
||||
|
||||
m_outputWheelSpeeds = null;
|
||||
m_outputDriveVoltages = null;
|
||||
|
||||
m_usePID = true;
|
||||
m_usePID = false;
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
@@ -165,8 +303,10 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
|
||||
* this is left to the user, since it is not appropriate for paths with non-stationary end-states.
|
||||
*
|
||||
* <p>Note2: The rotation controller will calculate the rotation based on the final pose
|
||||
* in the trajectory, not the poses at each time step.
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one of
|
||||
@@ -186,63 +326,31 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
|
||||
@SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
|
||||
public MecanumControllerCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
MecanumDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
|
||||
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
|
||||
m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
|
||||
m_kinematics = requireNonNullParam(kinematics,
|
||||
"kinematics", "MecanumControllerCommand");
|
||||
|
||||
m_xController = requireNonNullParam(xController,
|
||||
"xController", "MecanumControllerCommand");
|
||||
m_yController = requireNonNullParam(yController,
|
||||
"xController", "MecanumControllerCommand");
|
||||
m_thetaController = requireNonNullParam(thetaController,
|
||||
"thetaController", "MecanumControllerCommand");
|
||||
|
||||
m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
|
||||
"maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
|
||||
|
||||
m_frontLeftController = null;
|
||||
m_rearLeftController = null;
|
||||
m_frontRightController = null;
|
||||
m_rearRightController = null;
|
||||
|
||||
m_currentWheelSpeeds = null;
|
||||
|
||||
m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
|
||||
"outputWheelSpeeds", "MecanumControllerCommand");
|
||||
|
||||
m_outputDriveVoltages = null;
|
||||
|
||||
m_usePID = false;
|
||||
|
||||
addRequirements(requirements);
|
||||
Supplier<Pose2d> pose,
|
||||
MecanumDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
double maxWheelVelocityMetersPerSecond,
|
||||
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
|
||||
Subsystem... requirements) {
|
||||
this(trajectory, pose, kinematics, xController, yController, thetaController,
|
||||
() -> trajectory.getStates().get(trajectory.getStates().size() - 1)
|
||||
.poseMeters.getRotation(),
|
||||
maxWheelVelocityMetersPerSecond, outputWheelSpeeds, requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
var initialState = m_trajectory.sample(0);
|
||||
|
||||
// Sample final pose to get robot rotation
|
||||
m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
|
||||
|
||||
var initialXVelocity = initialState.velocityMetersPerSecond
|
||||
* initialState.poseMeters.getRotation().getCos();
|
||||
var initialYVelocity = initialState.velocityMetersPerSecond
|
||||
* initialState.poseMeters.getRotation().getSin();
|
||||
|
||||
m_prevSpeeds = m_kinematics.toWheelSpeeds(
|
||||
new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
|
||||
new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
|
||||
|
||||
m_timer.reset();
|
||||
m_timer.start();
|
||||
@@ -255,31 +363,9 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
double dt = curTime - m_prevTime;
|
||||
|
||||
var desiredState = m_trajectory.sample(curTime);
|
||||
var desiredPose = desiredState.poseMeters;
|
||||
|
||||
var poseError = desiredPose.relativeTo(m_pose.get());
|
||||
|
||||
double targetXVel = m_xController.calculate(
|
||||
m_pose.get().getX(),
|
||||
desiredPose.getX());
|
||||
|
||||
double targetYVel = m_yController.calculate(
|
||||
m_pose.get().getY(),
|
||||
desiredPose.getY());
|
||||
|
||||
// The robot will go to the desired rotation of the final pose in the trajectory,
|
||||
// not following the poses at individual states.
|
||||
double targetAngularVel = m_thetaController.calculate(
|
||||
m_pose.get().getRotation().getRadians(),
|
||||
m_finalPose.getRotation().getRadians());
|
||||
|
||||
double vRef = desiredState.velocityMetersPerSecond;
|
||||
|
||||
targetXVel += vRef * poseError.getRotation().getCos();
|
||||
targetYVel += vRef * poseError.getRotation().getSin();
|
||||
|
||||
var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
|
||||
|
||||
var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
|
||||
m_desiredRotation.get());
|
||||
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
|
||||
|
||||
targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
|
||||
@@ -287,7 +373,7 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
|
||||
var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
|
||||
var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
|
||||
var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
|
||||
var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
|
||||
|
||||
double frontLeftOutput;
|
||||
double rearLeftOutput;
|
||||
@@ -296,32 +382,32 @@ public class MecanumControllerCommand extends CommandBase {
|
||||
|
||||
if (m_usePID) {
|
||||
final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
|
||||
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
|
||||
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
|
||||
|
||||
final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
|
||||
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
|
||||
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
|
||||
|
||||
final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
|
||||
(frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
|
||||
(frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
|
||||
|
||||
final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
|
||||
(rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
|
||||
(rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
|
||||
|
||||
frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
|
||||
m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
|
||||
frontLeftSpeedSetpoint);
|
||||
m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
|
||||
frontLeftSpeedSetpoint);
|
||||
|
||||
rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
|
||||
m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
|
||||
rearLeftSpeedSetpoint);
|
||||
m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
|
||||
rearLeftSpeedSetpoint);
|
||||
|
||||
frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
|
||||
m_currentWheelSpeeds.get().frontRightMetersPerSecond,
|
||||
frontRightSpeedSetpoint);
|
||||
m_currentWheelSpeeds.get().frontRightMetersPerSecond,
|
||||
frontRightSpeedSetpoint);
|
||||
|
||||
rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
|
||||
m_currentWheelSpeeds.get().rearRightMetersPerSecond,
|
||||
rearRightSpeedSetpoint);
|
||||
m_currentWheelSpeeds.get().rearRightMetersPerSecond,
|
||||
rearRightSpeedSetpoint);
|
||||
|
||||
m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
|
||||
frontLeftOutput,
|
||||
|
||||
@@ -7,15 +7,15 @@
|
||||
|
||||
package edu.wpi.first.wpilibj2.command;
|
||||
|
||||
|
||||
import java.util.function.Consumer;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.wpilibj.trajectory.Trajectory;
|
||||
@@ -38,15 +38,12 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
@SuppressWarnings("MemberName")
|
||||
public class SwerveControllerCommand extends CommandBase {
|
||||
private final Timer m_timer = new Timer();
|
||||
private Pose2d m_finalPose;
|
||||
|
||||
private final Trajectory m_trajectory;
|
||||
private final Supplier<Pose2d> m_pose;
|
||||
private final SwerveDriveKinematics m_kinematics;
|
||||
private final PIDController m_xController;
|
||||
private final PIDController m_yController;
|
||||
private final ProfiledPIDController m_thetaController;
|
||||
private final HolonomicDriveController m_controller;
|
||||
private final Consumer<SwerveModuleState[]> m_outputModuleStates;
|
||||
private final Supplier<Rotation2d> m_desiredRotation;
|
||||
|
||||
/**
|
||||
* Constructs a new SwerveControllerCommand that when executed will follow the provided
|
||||
@@ -56,8 +53,66 @@ public class SwerveControllerCommand extends CommandBase {
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
|
||||
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The rotation controller will calculate the rotation based on the final pose
|
||||
* in the trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one of
|
||||
* the odometry classes to provide this.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the drivetrain should be facing. This
|
||||
* is sampled at each time step.
|
||||
* @param outputModuleStates The raw output module states from the
|
||||
* position controllers.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public SwerveControllerCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
SwerveDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
Supplier<Rotation2d> desiredRotation,
|
||||
Consumer<SwerveModuleState[]> outputModuleStates,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
|
||||
m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
|
||||
|
||||
m_controller = new HolonomicDriveController(
|
||||
requireNonNullParam(xController,
|
||||
"xController", "SwerveControllerCommand"),
|
||||
requireNonNullParam(yController,
|
||||
"xController", "SwerveControllerCommand"),
|
||||
requireNonNullParam(thetaController,
|
||||
"thetaController", "SwerveControllerCommand")
|
||||
);
|
||||
|
||||
m_outputModuleStates = requireNonNullParam(outputModuleStates,
|
||||
"frontLeftOutput", "SwerveControllerCommand");
|
||||
|
||||
m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
|
||||
"SwerveControllerCommand");
|
||||
|
||||
addRequirements(requirements);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a new SwerveControllerCommand that when executed will follow the provided
|
||||
* trajectory. This command will not return output voltages but rather raw module states from the
|
||||
* position controllers which need to be put into a velocity PID.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
|
||||
* this is left to the user, since it is not appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one of
|
||||
@@ -73,38 +128,23 @@ public class SwerveControllerCommand extends CommandBase {
|
||||
* position controllers.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
public SwerveControllerCommand(Trajectory trajectory,
|
||||
Supplier<Pose2d> pose,
|
||||
SwerveDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
|
||||
Consumer<SwerveModuleState[]> outputModuleStates,
|
||||
Subsystem... requirements) {
|
||||
m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
|
||||
m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
|
||||
m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
|
||||
|
||||
m_xController = requireNonNullParam(xController,
|
||||
"xController", "SwerveControllerCommand");
|
||||
m_yController = requireNonNullParam(yController,
|
||||
"xController", "SwerveControllerCommand");
|
||||
m_thetaController = requireNonNullParam(thetaController,
|
||||
"thetaController", "SwerveControllerCommand");
|
||||
|
||||
m_outputModuleStates = requireNonNullParam(outputModuleStates,
|
||||
"frontLeftOutput", "SwerveControllerCommand");
|
||||
addRequirements(requirements);
|
||||
Supplier<Pose2d> pose,
|
||||
SwerveDriveKinematics kinematics,
|
||||
PIDController xController,
|
||||
PIDController yController,
|
||||
ProfiledPIDController thetaController,
|
||||
Consumer<SwerveModuleState[]> outputModuleStates,
|
||||
Subsystem... requirements) {
|
||||
this(trajectory, pose, kinematics, xController, yController, thetaController,
|
||||
() -> trajectory.getStates().get(trajectory.getStates().size() - 1)
|
||||
.poseMeters.getRotation(),
|
||||
outputModuleStates, requirements);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
// Sample final pose to get robot rotation
|
||||
m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
|
||||
|
||||
m_timer.reset();
|
||||
m_timer.start();
|
||||
}
|
||||
@@ -113,37 +153,13 @@ public class SwerveControllerCommand extends CommandBase {
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void execute() {
|
||||
double curTime = m_timer.get();
|
||||
|
||||
var desiredState = m_trajectory.sample(curTime);
|
||||
var desiredPose = desiredState.poseMeters;
|
||||
|
||||
var poseError = desiredPose.relativeTo(m_pose.get());
|
||||
|
||||
double targetXVel = m_xController.calculate(
|
||||
m_pose.get().getX(),
|
||||
desiredPose.getX());
|
||||
|
||||
double targetYVel = m_yController.calculate(
|
||||
m_pose.get().getY(),
|
||||
desiredPose.getY());
|
||||
|
||||
// The robot will go to the desired rotation of the final pose in the trajectory,
|
||||
// not following the poses at individual states.
|
||||
double targetAngularVel = m_thetaController.calculate(
|
||||
m_pose.get().getRotation().getRadians(),
|
||||
m_finalPose.getRotation().getRadians());
|
||||
|
||||
double vRef = desiredState.velocityMetersPerSecond;
|
||||
|
||||
targetXVel += vRef * poseError.getRotation().getCos();
|
||||
targetYVel += vRef * poseError.getRotation().getSin();
|
||||
|
||||
var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
|
||||
|
||||
var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
|
||||
m_desiredRotation.get());
|
||||
var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
|
||||
|
||||
m_outputModuleStates.accept(targetModuleStates);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -16,6 +16,7 @@ MecanumControllerCommand::MecanumControllerCommand(
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
@@ -30,11 +31,85 @@ MecanumControllerCommand::MecanumControllerCommand(
|
||||
m_pose(pose),
|
||||
m_feedforward(feedforward),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_desiredRotation(desiredRotation),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_frontLeftController(
|
||||
std::make_unique<frc2::PIDController>(frontLeftController)),
|
||||
m_rearLeftController(
|
||||
std::make_unique<frc2::PIDController>(rearLeftController)),
|
||||
m_frontRightController(
|
||||
std::make_unique<frc2::PIDController>(frontRightController)),
|
||||
m_rearRightController(
|
||||
std::make_unique<frc2::PIDController>(rearRightController)),
|
||||
m_currentWheelSpeeds(currentWheelSpeeds),
|
||||
m_outputVolts(output),
|
||||
m_usePID(true) {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
MecanumControllerCommand::MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SimpleMotorFeedforward<units::meters> feedforward,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
frc2::PIDController rearLeftController,
|
||||
frc2::PIDController frontRightController,
|
||||
frc2::PIDController rearRightController,
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_feedforward(feedforward),
|
||||
m_kinematics(kinematics),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_frontLeftController(
|
||||
std::make_unique<frc2::PIDController>(frontLeftController)),
|
||||
m_rearLeftController(
|
||||
std::make_unique<frc2::PIDController>(rearLeftController)),
|
||||
m_frontRightController(
|
||||
std::make_unique<frc2::PIDController>(frontRightController)),
|
||||
m_rearRightController(
|
||||
std::make_unique<frc2::PIDController>(rearRightController)),
|
||||
m_currentWheelSpeeds(currentWheelSpeeds),
|
||||
m_outputVolts(output),
|
||||
m_usePID(true) {
|
||||
AddRequirements(requirements);
|
||||
m_desiredRotation = [&] {
|
||||
return m_trajectory.States().back().pose.Rotation();
|
||||
};
|
||||
}
|
||||
|
||||
MecanumControllerCommand::MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SimpleMotorFeedforward<units::meters> feedforward,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
frc2::PIDController rearLeftController,
|
||||
frc2::PIDController frontRightController,
|
||||
frc2::PIDController rearRightController,
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
output,
|
||||
wpi::ArrayRef<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_feedforward(feedforward),
|
||||
m_kinematics(kinematics),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_desiredRotation(desiredRotation),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_frontLeftController(
|
||||
std::make_unique<frc2::PIDController>(frontLeftController)),
|
||||
@@ -70,11 +145,7 @@ MecanumControllerCommand::MecanumControllerCommand(
|
||||
m_pose(pose),
|
||||
m_feedforward(feedforward),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_frontLeftController(
|
||||
std::make_unique<frc2::PIDController>(frontLeftController)),
|
||||
@@ -88,6 +159,31 @@ MecanumControllerCommand::MecanumControllerCommand(
|
||||
m_outputVolts(output),
|
||||
m_usePID(true) {
|
||||
AddRequirements(requirements);
|
||||
m_desiredRotation = [&] {
|
||||
return m_trajectory.States().back().pose.Rotation();
|
||||
};
|
||||
}
|
||||
|
||||
MecanumControllerCommand::MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t, units::meters_per_second_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_desiredRotation(desiredRotation),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_outputVel(output),
|
||||
m_usePID(false) {
|
||||
AddRequirements(requirements);
|
||||
}
|
||||
|
||||
MecanumControllerCommand::MecanumControllerCommand(
|
||||
@@ -103,11 +199,32 @@ MecanumControllerCommand::MecanumControllerCommand(
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_outputVel(output),
|
||||
m_usePID(false) {
|
||||
AddRequirements(requirements);
|
||||
m_desiredRotation = [&] {
|
||||
return m_trajectory.States().back().pose.Rotation();
|
||||
};
|
||||
}
|
||||
|
||||
MecanumControllerCommand::MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t, units::meters_per_second_t)>
|
||||
output,
|
||||
wpi::ArrayRef<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_desiredRotation(desiredRotation),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_outputVel(output),
|
||||
m_usePID(false) {
|
||||
@@ -127,15 +244,14 @@ MecanumControllerCommand::MecanumControllerCommand(
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_maxWheelVelocity(maxWheelVelocity),
|
||||
m_outputVel(output),
|
||||
m_usePID(false) {
|
||||
AddRequirements(requirements);
|
||||
m_desiredRotation = [&] {
|
||||
return m_trajectory.States().back().pose.Rotation();
|
||||
};
|
||||
}
|
||||
|
||||
void MecanumControllerCommand::Initialize() {
|
||||
@@ -165,29 +281,9 @@ void MecanumControllerCommand::Execute() {
|
||||
auto dt = curTime - m_prevTime;
|
||||
|
||||
auto m_desiredState = m_trajectory.Sample(curTime);
|
||||
auto m_desiredPose = m_desiredState.pose;
|
||||
|
||||
auto m_poseError = m_desiredPose.RelativeTo(m_pose());
|
||||
|
||||
auto targetXVel = meters_per_second_t(m_xController->Calculate(
|
||||
(m_pose().X().to<double>()), (m_desiredPose.X().to<double>())));
|
||||
auto targetYVel = meters_per_second_t(m_yController->Calculate(
|
||||
(m_pose().Y().to<double>()), (m_desiredPose.Y().to<double>())));
|
||||
|
||||
// Profiled PID Controller only takes meters as setpoint and measurement
|
||||
// The robot will go to the desired rotation of the final pose in the
|
||||
// trajectory, not following the poses at individual states.
|
||||
auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate(
|
||||
m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
|
||||
|
||||
auto vRef = m_desiredState.velocity;
|
||||
|
||||
targetXVel += vRef * m_poseError.Rotation().Cos();
|
||||
targetYVel += vRef * m_poseError.Rotation().Sin();
|
||||
|
||||
auto targetChassisSpeeds =
|
||||
frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
|
||||
|
||||
m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
|
||||
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
|
||||
|
||||
targetWheelSpeeds.Normalize(m_maxWheelVelocity);
|
||||
|
||||
@@ -10,6 +10,7 @@
|
||||
#include <initializer_list>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/controller/HolonomicDriveController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
@@ -63,8 +64,61 @@ class MecanumControllerCommand
|
||||
* completion of the path this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The rotation controller will calculate the rotation based on the
|
||||
* final pose in the trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
* provided by the odometry class.
|
||||
* @param feedforward The feedforward to use for the drivetrain.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the robot should be facing.
|
||||
* This is sampled at each time step.
|
||||
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
|
||||
* @param frontLeftController The front left wheel velocity PID.
|
||||
* @param rearLeftController The rear left wheel velocity PID.
|
||||
* @param frontRightController The front right wheel velocity PID.
|
||||
* @param rearRightController The rear right wheel velocity PID.
|
||||
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
|
||||
* the current wheel speeds.
|
||||
* @param output The output of the velocity PIDs.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SimpleMotorFeedforward<units::meters> feedforward,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
frc2::PIDController rearLeftController,
|
||||
frc2::PIDController frontRightController,
|
||||
frc2::PIDController rearRightController,
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow
|
||||
* the provided trajectory. PID control and feedforward are handled
|
||||
* internally. Outputs are scaled from -12 to 12 as a voltage output to the
|
||||
* motor.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon
|
||||
* completion of the path this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
@@ -114,8 +168,61 @@ class MecanumControllerCommand
|
||||
* completion of the path this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The rotation controller will calculate the rotation based on the
|
||||
* final pose in the trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
* provided by the odometry class.
|
||||
* @param feedforward The feedforward to use for the drivetrain.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the robot should be facing.
|
||||
* This is sampled at each time step.
|
||||
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
|
||||
* @param frontLeftController The front left wheel velocity PID.
|
||||
* @param rearLeftController The rear left wheel velocity PID.
|
||||
* @param frontRightController The front right wheel velocity PID.
|
||||
* @param rearRightController The rear right wheel velocity PID.
|
||||
* @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing
|
||||
* the current wheel speeds.
|
||||
* @param output The output of the velocity PIDs.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SimpleMotorFeedforward<units::meters> feedforward,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
|
||||
frc2::PIDController frontLeftController,
|
||||
frc2::PIDController rearLeftController,
|
||||
frc2::PIDController frontRightController,
|
||||
frc2::PIDController rearRightController,
|
||||
std::function<void(units::volt_t, units::volt_t, units::volt_t,
|
||||
units::volt_t)>
|
||||
output,
|
||||
wpi::ArrayRef<Subsystem*> requirements = {});
|
||||
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow
|
||||
* the provided trajectory. PID control and feedforward are handled
|
||||
* internally. Outputs are scaled from -12 to 12 as a voltage output to the
|
||||
* motor.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon
|
||||
* completion of the path this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
@@ -164,8 +271,48 @@ class MecanumControllerCommand
|
||||
* completion of the path - this is left to the user, since it is not
|
||||
* appropriate for paths with non-stationary end-states.
|
||||
*
|
||||
* <p>Note2: The rotation controller will calculate the rotation based on the
|
||||
* final pose in the trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one
|
||||
* of the odometry classes to provide this.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the robot should be facing.
|
||||
* This is sampled at each time step.
|
||||
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
|
||||
* @param output The output of the position PIDs.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t,
|
||||
units::meters_per_second_t)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow
|
||||
* the provided trajectory. The user should implement a velocity PID on the
|
||||
* desired output wheel velocities.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon
|
||||
* completion of the path - this is left to the user, since it is not
|
||||
* appropriate for paths with non-stationary end-states.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one
|
||||
@@ -202,8 +349,48 @@ class MecanumControllerCommand
|
||||
* completion of the path - this is left to the user, since it is not
|
||||
* appropriate for paths with non-stationary end-states.
|
||||
*
|
||||
* <p>Note2: The rotation controller will calculate the rotation based on the
|
||||
* final pose in the trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one
|
||||
* of the odometry classes to provide this.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the robot should be facing.
|
||||
* This is sampled at every time step.
|
||||
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
|
||||
* @param output The output of the position PIDs.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
MecanumControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
|
||||
frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
units::meters_per_second_t maxWheelVelocity,
|
||||
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
|
||||
units::meters_per_second_t,
|
||||
units::meters_per_second_t)>
|
||||
output,
|
||||
wpi::ArrayRef<Subsystem*> requirements = {});
|
||||
|
||||
/**
|
||||
* Constructs a new MecanumControllerCommand that when executed will follow
|
||||
* the provided trajectory. The user should implement a velocity PID on the
|
||||
* desired output wheel velocities.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon
|
||||
* completion of the path - this is left to the user, since it is not
|
||||
* appropriate for paths with non-stationary end-states.
|
||||
*
|
||||
* <p>Note2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose - use one
|
||||
@@ -244,9 +431,8 @@ class MecanumControllerCommand
|
||||
std::function<frc::Pose2d()> m_pose;
|
||||
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
|
||||
frc::MecanumDriveKinematics m_kinematics;
|
||||
std::unique_ptr<frc2::PIDController> m_xController;
|
||||
std::unique_ptr<frc2::PIDController> m_yController;
|
||||
std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
|
||||
frc::HolonomicDriveController m_controller;
|
||||
std::function<frc::Rotation2d()> m_desiredRotation;
|
||||
const units::meters_per_second_t m_maxWheelVelocity;
|
||||
std::unique_ptr<frc2::PIDController> m_frontLeftController;
|
||||
std::unique_ptr<frc2::PIDController> m_rearLeftController;
|
||||
@@ -264,6 +450,5 @@ class MecanumControllerCommand
|
||||
frc2::Timer m_timer;
|
||||
frc::MecanumDriveWheelSpeeds m_prevSpeeds;
|
||||
units::second_t m_prevTime;
|
||||
frc::Pose2d m_finalPose;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
@@ -10,6 +10,7 @@
|
||||
#include <initializer_list>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/controller/HolonomicDriveController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
@@ -70,8 +71,46 @@ class SwerveControllerCommand
|
||||
* completion of the path- this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The rotation controller will calculate the rotation based on the
|
||||
* final pose in the trajectory, not the poses at each time step.
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
* provided by the odometry class.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the drivetrain should be
|
||||
* facing. This is sampled at each time step.
|
||||
* @param output The raw output module states from the
|
||||
* position controllers.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
SwerveControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
|
||||
output,
|
||||
std::initializer_list<Subsystem*> requirements);
|
||||
|
||||
/**
|
||||
* Constructs a new SwerveControllerCommand that when executed will follow the
|
||||
* provided trajectory. This command will not return output voltages but
|
||||
* rather raw module states from the position controllers which need to be put
|
||||
* into a velocity PID.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon
|
||||
* completion of the path- this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
@@ -106,8 +145,47 @@ class SwerveControllerCommand
|
||||
* completion of the path- this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The rotation controller will calculate the rotation based on the
|
||||
* final pose in the trajectory, not the poses at each time step.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
* provided by the odometry class.
|
||||
* @param kinematics The kinematics for the robot drivetrain.
|
||||
* @param xController The Trajectory Tracker PID controller
|
||||
* for the robot's x position.
|
||||
* @param yController The Trajectory Tracker PID controller
|
||||
* for the robot's y position.
|
||||
* @param thetaController The Trajectory Tracker PID controller
|
||||
* for angle for the robot.
|
||||
* @param desiredRotation The angle that the drivetrain should be
|
||||
* facing. This is sampled at each time step.
|
||||
* @param output The raw output module states from the
|
||||
* position controllers.
|
||||
* @param requirements The subsystems to require.
|
||||
*/
|
||||
SwerveControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
|
||||
output,
|
||||
wpi::ArrayRef<Subsystem*> requirements = {});
|
||||
|
||||
/**
|
||||
* Constructs a new SwerveControllerCommand that when executed will follow the
|
||||
* provided trajectory. This command will not return output voltages but
|
||||
* rather raw module states from the position controllers which need to be put
|
||||
* into a velocity PID.
|
||||
*
|
||||
* <p>Note: The controllers will *not* set the outputVolts to zero upon
|
||||
* completion of the path- this is left to the user, since it is not
|
||||
* appropriate for paths with nonstationary endstates.
|
||||
*
|
||||
* <p>Note 2: The final rotation of the robot will be set to the rotation of
|
||||
* the final pose in the trajectory. The robot will not follow the rotations
|
||||
* from the poses at each timestep. If alternate rotation behavior is desired,
|
||||
* the other constructor with a supplier for rotation should be used.
|
||||
*
|
||||
* @param trajectory The trajectory to follow.
|
||||
* @param pose A function that supplies the robot pose,
|
||||
@@ -144,15 +222,15 @@ class SwerveControllerCommand
|
||||
frc::Trajectory m_trajectory;
|
||||
std::function<frc::Pose2d()> m_pose;
|
||||
frc::SwerveDriveKinematics<NumModules> m_kinematics;
|
||||
std::unique_ptr<frc2::PIDController> m_xController;
|
||||
std::unique_ptr<frc2::PIDController> m_yController;
|
||||
std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
|
||||
frc::HolonomicDriveController m_controller;
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
|
||||
m_outputStates;
|
||||
|
||||
std::function<frc::Rotation2d()> m_desiredRotation;
|
||||
|
||||
frc2::Timer m_timer;
|
||||
units::second_t m_prevTime;
|
||||
frc::Pose2d m_finalPose;
|
||||
frc::Rotation2d m_finalRotation;
|
||||
};
|
||||
} // namespace frc2
|
||||
|
||||
|
||||
@@ -17,16 +17,51 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_desiredRotation(desiredRotation),
|
||||
m_outputStates(output) {
|
||||
this->AddRequirements(requirements);
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
|
||||
std::initializer_list<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_outputStates(output) {
|
||||
this->AddRequirements(requirements);
|
||||
m_desiredRotation = [&] {
|
||||
return m_trajectory.States().back().pose.Rotation();
|
||||
};
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
|
||||
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
|
||||
frc::SwerveDriveKinematics<NumModules> kinematics,
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
frc::ProfiledPIDController<units::radians> thetaController,
|
||||
std::function<frc::Rotation2d()> desiredRotation,
|
||||
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
|
||||
wpi::ArrayRef<Subsystem*> requirements)
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_desiredRotation(desiredRotation),
|
||||
m_outputStates(output) {
|
||||
this->AddRequirements(requirements);
|
||||
}
|
||||
@@ -42,19 +77,16 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
|
||||
: m_trajectory(trajectory),
|
||||
m_pose(pose),
|
||||
m_kinematics(kinematics),
|
||||
m_xController(std::make_unique<frc2::PIDController>(xController)),
|
||||
m_yController(std::make_unique<frc2::PIDController>(yController)),
|
||||
m_thetaController(
|
||||
std::make_unique<frc::ProfiledPIDController<units::radians>>(
|
||||
thetaController)),
|
||||
m_controller(xController, yController, thetaController),
|
||||
m_outputStates(output) {
|
||||
this->AddRequirements(requirements);
|
||||
m_desiredRotation = [&] {
|
||||
return m_trajectory.States().back().pose.Rotation();
|
||||
};
|
||||
}
|
||||
|
||||
template <size_t NumModules>
|
||||
void SwerveControllerCommand<NumModules>::Initialize() {
|
||||
m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
|
||||
|
||||
m_timer.Reset();
|
||||
m_timer.Start();
|
||||
}
|
||||
@@ -62,34 +94,10 @@ void SwerveControllerCommand<NumModules>::Initialize() {
|
||||
template <size_t NumModules>
|
||||
void SwerveControllerCommand<NumModules>::Execute() {
|
||||
auto curTime = units::second_t(m_timer.Get());
|
||||
|
||||
auto m_desiredState = m_trajectory.Sample(curTime);
|
||||
auto m_desiredPose = m_desiredState.pose;
|
||||
|
||||
auto m_poseError = m_desiredPose.RelativeTo(m_pose());
|
||||
|
||||
auto targetXVel = units::meters_per_second_t(
|
||||
m_xController->Calculate((m_pose().X().template to<double>()),
|
||||
(m_desiredPose.X().template to<double>())));
|
||||
auto targetYVel = units::meters_per_second_t(
|
||||
m_yController->Calculate((m_pose().Y().template to<double>()),
|
||||
(m_desiredPose.Y().template to<double>())));
|
||||
|
||||
// Profiled PID Controller only takes meters as setpoint and measurement
|
||||
// The robot will go to the desired rotation of the final pose in the
|
||||
// trajectory, not following the poses at individual states.
|
||||
auto targetAngularVel =
|
||||
units::radians_per_second_t(m_thetaController->Calculate(
|
||||
m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
|
||||
|
||||
auto vRef = m_desiredState.velocity;
|
||||
|
||||
targetXVel += vRef * m_poseError.Rotation().Cos();
|
||||
targetYVel += vRef * m_poseError.Rotation().Sin();
|
||||
|
||||
auto targetChassisSpeeds =
|
||||
frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
|
||||
|
||||
m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
|
||||
auto targetModuleStates =
|
||||
m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user