Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)

Browse Source
This commit is contained in:
Gold856
2025-08-01 02:05:42 -04:00
committed by GitHub
parent b251d16ef7
commit e0e774abde
51 changed files with 0 additions and 5642 deletions
Download Patch File Download Diff File Expand all files Collapse all files

576
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -1,576 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Consumer;
import java.util.function.Supplier;
/**
* A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
* ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a mecanum drive.
*
* <p>The command handles trajectory-following, Velocity PID calculations, and feedforwards
* internally. This is intended to be a more-or-less "complete solution" that can be used by teams
* without a great deal of controls expertise.
*
* <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
* functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
* and feedforward functionality, returning only the raw wheel speeds from the PID controllers.
*
* <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
* to the angle given in the final state of the trajectory.
*
* <p>This class is provided by the NewCommands VendorDep
*/
@SuppressWarnings("removal")
public class MecanumControllerCommand extends Command {
private final Timer m_timer = new Timer();
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 HolonomicDriveController m_controller;
private final Supplier<Rotation2d> m_desiredRotation;
private final double m_maxWheelVelocity;
private final PIDController m_frontLeftController;
private final PIDController m_rearLeftController;
private final PIDController m_frontRightController;
private final PIDController m_rearRightController;
private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
private final MecanumVoltagesConsumer m_outputDriveVoltages;
private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
private double m_prevFrontLeftSpeedSetpoint; // m/s
private double m_prevRearLeftSpeedSetpoint; // m/s
private double m_prevFrontRightSpeedSetpoint; // m/s
private double m_prevRearRightSpeedSetpoint; // m/s
/**
* 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.
*
* @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 maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s.
* @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 MecanumVoltagesConsumer that consumes voltages of mecanum motors.
* @param requirements The subsystems to require.
*/
@SuppressWarnings("this-escape")
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SimpleMotorFeedforward feedforward,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocity,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
PIDController rearRightController,
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
MecanumVoltagesConsumer 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", "MecanumControllerCommand"),
requireNonNullParam(yController, "yController", "MecanumControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "MecanumControllerCommand"));
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
m_maxWheelVelocity = maxWheelVelocity;
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.
*
* @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.
* @deprecated Use {@link MecanumVoltagesConsumer} instead of {@code
* Consumer<MecanumDriveMotorVoltages}.
*/
@Deprecated(since = "2025", forRemoval = true)
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) {
this(
trajectory,
pose,
feedforward,
kinematics,
xController,
yController,
thetaController,
desiredRotation,
maxWheelVelocityMetersPerSecond,
frontLeftController,
rearLeftController,
frontRightController,
rearRightController,
currentWheelSpeeds,
(frontLeft, frontRight, rearLeft, rearRight) ->
outputDriveVoltages.accept(
new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)),
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 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 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 MecanumVoltagesConsumer that consumes voltages of mecanum motors.
* @param requirements The subsystems to require.
*/
public MecanumControllerCommand(
Trajectory trajectory,
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,
MecanumVoltagesConsumer outputDriveVoltages,
Subsystem... requirements) {
this(
trajectory,
pose,
feedforward,
kinematics,
xController,
yController,
thetaController,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
maxWheelVelocityMetersPerSecond,
frontLeftController,
rearLeftController,
frontRightController,
rearRightController,
currentWheelSpeeds,
outputDriveVoltages,
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 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 maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s.
* @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.
* @deprecated Use {@link MecanumVoltagesConsumer} instead of {@code
* Consumer<MecanumDriveMotorVoltages>}.
*/
@Deprecated(since = "2025", forRemoval = true)
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SimpleMotorFeedforward feedforward,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocity,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
PIDController rearRightController,
Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
Subsystem... requirements) {
this(
trajectory,
pose,
feedforward,
kinematics,
xController,
yController,
thetaController,
maxWheelVelocity,
frontLeftController,
rearLeftController,
frontRightController,
rearRightController,
currentWheelSpeeds,
(frontLeft, frontRight, rearLeft, rearRight) ->
outputDriveVoltages.accept(
new MecanumDriveMotorVoltages(frontLeft, frontRight, rearLeft, rearRight)),
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 nonstationary 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 maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
@SuppressWarnings("this-escape")
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocity,
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_controller =
new HolonomicDriveController(
requireNonNullParam(xController, "xController", "MecanumControllerCommand"),
requireNonNullParam(yController, "yController", "MecanumControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "MecanumControllerCommand"));
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
m_maxWheelVelocity = maxWheelVelocity;
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);
}
/**
* 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 nonstationary 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 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 maxWheelVelocity The maximum velocity of a drivetrain wheel.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
MecanumDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocity,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
this(
trajectory,
pose,
kinematics,
xController,
yController,
thetaController,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
maxWheelVelocity,
outputWheelSpeeds,
requirements);
}
@Override
public void initialize() {
var initialState = m_trajectory.sample(0);
var initialXVelocity = initialState.velocity * initialState.pose.getRotation().getCos();
var initialYVelocity = initialState.velocity * initialState.pose.getRotation().getSin();
MecanumDriveWheelSpeeds prevSpeeds =
m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeft;
m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeft;
m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRight;
m_prevRearRightSpeedSetpoint = prevSpeeds.rearRight;
m_timer.restart();
}
@Override
public void execute() {
double curTime = m_timer.get();
var desiredState = m_trajectory.sample(curTime);
var targetChassisSpeeds =
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds = targetWheelSpeeds.desaturate(m_maxWheelVelocity);
double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
double frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
double rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
double frontLeftOutput;
double rearLeftOutput;
double frontRightOutput;
double rearRightOutput;
if (m_usePID) {
final double frontLeftFeedforward =
m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint);
final double rearLeftFeedforward =
m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint);
final double frontRightFeedforward =
m_feedforward.calculate(m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint);
final double rearRightFeedforward =
m_feedforward.calculate(m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint);
frontLeftOutput =
frontLeftFeedforward
+ m_frontLeftController.calculate(
m_currentWheelSpeeds.get().frontLeft, frontLeftSpeedSetpoint);
rearLeftOutput =
rearLeftFeedforward
+ m_rearLeftController.calculate(
m_currentWheelSpeeds.get().rearLeft, rearLeftSpeedSetpoint);
frontRightOutput =
frontRightFeedforward
+ m_frontRightController.calculate(
m_currentWheelSpeeds.get().frontRight, frontRightSpeedSetpoint);
rearRightOutput =
rearRightFeedforward
+ m_rearRightController.calculate(
m_currentWheelSpeeds.get().rearRight, rearRightSpeedSetpoint);
m_outputDriveVoltages.accept(
frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput);
} else {
m_outputWheelSpeeds.accept(
new MecanumDriveWheelSpeeds(
frontLeftSpeedSetpoint,
frontRightSpeedSetpoint,
rearLeftSpeedSetpoint,
rearRightSpeedSetpoint));
}
}
@Override
public void end(boolean interrupted) {
m_timer.stop();
}
@Override
public boolean isFinished() {
return m_timer.hasElapsed(m_trajectory.getTotalTime());
}
/** A consumer to represent an operation on the voltages of a mecanum drive. */
@FunctionalInterface
public interface MecanumVoltagesConsumer {
/**
* Accepts the voltages to perform some operation with them.
*
* @param frontLeftVoltage The voltage of the front left motor.
* @param frontRightVoltage The voltage of the front right motor.
* @param rearLeftVoltage The voltage of the rear left motor.
* @param rearRightVoltage The voltage of the rear left motor.
*/
void accept(
double frontLeftVoltage,
double frontRightVoltage,
double rearLeftVoltage,
double rearRightVoltage);
}
}

236
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
View File

@@ -1,236 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Consumer;
import java.util.function.Supplier;
/**
* A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
* ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a swerve drive.
*
* <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState}) in an
* array. The desired wheel and module rotation velocities should be taken from those and used in
* velocity PIDs.
*
* <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
* to the angle given in the final state of the trajectory.
*
* <p>This class is provided by the NewCommands VendorDep
*/
public class SwerveControllerCommand extends Command {
private final Timer m_timer = new Timer();
private final Trajectory m_trajectory;
private final Supplier<Pose2d> m_pose;
private final SwerveDriveKinematics m_kinematics;
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
* 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 to do since it is not appropriate for paths with nonstationary
* endstates.
*
* @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.
*/
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
this(
trajectory,
pose,
kinematics,
new HolonomicDriveController(
requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
requireNonNullParam(yController, "yController", "SwerveControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")),
desiredRotation,
outputModuleStates,
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 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 outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
public SwerveControllerCommand(
Trajectory trajectory,
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).pose.getRotation(),
outputModuleStates,
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 the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the drivetrain.
* @param outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
HolonomicDriveController controller,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
this(
trajectory,
pose,
kinematics,
controller,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
outputModuleStates,
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.
*
* @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 controller The HolonomicDriveController for the drivetrain.
* @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("this-escape")
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
HolonomicDriveController controller,
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 = requireNonNullParam(controller, "controller", "SwerveControllerCommand");
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
m_outputModuleStates =
requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand");
addRequirements(requirements);
}
@Override
public void initialize() {
m_timer.restart();
}
@Override
public void execute() {
double curTime = m_timer.get();
var desiredState = m_trajectory.sample(curTime);
var targetChassisSpeeds =
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
m_outputModuleStates.accept(targetModuleStates);
}
@Override
public void end(boolean interrupted) {
m_timer.stop();
}
@Override
public boolean isFinished() {
return m_timer.hasElapsed(m_trajectory.getTotalTime());
}
}

224
wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
View File

@@ -1,224 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc2/command/MecanumControllerCommand.h"
#include <memory>
#include <utility>
#include <units/velocity.h>
#include <units/voltage.h>
using namespace frc2;
using kv_unit = units::compound_unit<units::volts,
units::inverse<units::meters_per_second>>;
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::MecanumDriveKinematics kinematics, frc::PIDController xController,
frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
std::function<frc::Rotation2d()> desiredRotation,
units::meters_per_second_t maxWheelVelocity,
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
frc::PIDController frontLeftController,
frc::PIDController rearLeftController,
frc::PIDController frontRightController,
frc::PIDController rearRightController,
std::function<void(units::volt_t, units::volt_t, units::volt_t,
units::volt_t)>
output,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_feedforward(feedforward),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_desiredRotation(std::move(desiredRotation)),
m_maxWheelVelocity(maxWheelVelocity),
m_frontLeftController(
std::make_unique<frc::PIDController>(frontLeftController)),
m_rearLeftController(
std::make_unique<frc::PIDController>(rearLeftController)),
m_frontRightController(
std::make_unique<frc::PIDController>(frontRightController)),
m_rearRightController(
std::make_unique<frc::PIDController>(rearRightController)),
m_currentWheelSpeeds(std::move(currentWheelSpeeds)),
m_outputVolts(std::move(output)),
m_usePID(true) {
AddRequirements(requirements);
}
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SimpleMotorFeedforward<units::meters> feedforward,
frc::MecanumDriveKinematics kinematics, frc::PIDController xController,
frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
units::meters_per_second_t maxWheelVelocity,
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
frc::PIDController frontLeftController,
frc::PIDController rearLeftController,
frc::PIDController frontRightController,
frc::PIDController rearRightController,
std::function<void(units::volt_t, units::volt_t, units::volt_t,
units::volt_t)>
output,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_feedforward(feedforward),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_maxWheelVelocity(maxWheelVelocity),
m_frontLeftController(
std::make_unique<frc::PIDController>(frontLeftController)),
m_rearLeftController(
std::make_unique<frc::PIDController>(rearLeftController)),
m_frontRightController(
std::make_unique<frc::PIDController>(frontRightController)),
m_rearRightController(
std::make_unique<frc::PIDController>(rearRightController)),
m_currentWheelSpeeds(std::move(currentWheelSpeeds)),
m_outputVolts(std::move(output)),
m_usePID(true) {
AddRequirements(requirements);
}
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::MecanumDriveKinematics kinematics, frc::PIDController xController,
frc::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,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_feedforward(0_V, units::unit_t<kv_unit>{0}),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_desiredRotation(std::move(desiredRotation)),
m_maxWheelVelocity(maxWheelVelocity),
m_outputVel(std::move(output)),
m_usePID(false) {
AddRequirements(requirements);
}
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::MecanumDriveKinematics kinematics, frc::PIDController xController,
frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
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,
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_feedforward(0_V, units::unit_t<kv_unit>{0}),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_maxWheelVelocity(maxWheelVelocity),
m_outputVel(std::move(output)),
m_usePID(false) {
AddRequirements(requirements);
}
void MecanumControllerCommand::Initialize() {
if (m_desiredRotation == nullptr) {
m_desiredRotation = [&] {
return m_trajectory.States().back().pose.Rotation();
};
}
m_prevTime = 0_s;
auto initialState = m_trajectory.Sample(0_s);
auto initialXVelocity =
initialState.velocity * initialState.pose.Rotation().Cos();
auto initialYVelocity =
initialState.velocity * initialState.pose.Rotation().Sin();
m_prevSpeeds = m_kinematics.ToWheelSpeeds(
frc::ChassisSpeeds{initialXVelocity, initialYVelocity, 0_rad_per_s});
m_timer.Restart();
if (m_usePID) {
m_frontLeftController->Reset();
m_rearLeftController->Reset();
m_frontRightController->Reset();
m_rearRightController->Reset();
}
}
void MecanumControllerCommand::Execute() {
auto curTime = m_timer.Get();
auto m_desiredState = m_trajectory.Sample(curTime);
auto targetChassisSpeeds =
m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds = targetWheelSpeeds.Desaturate(m_maxWheelVelocity);
auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
auto frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
if (m_usePID) {
auto frontLeftFeedforward =
m_feedforward.Calculate(m_prevSpeeds.frontLeft, frontLeftSpeedSetpoint);
auto rearLeftFeedforward =
m_feedforward.Calculate(m_prevSpeeds.rearLeft, rearLeftSpeedSetpoint);
auto frontRightFeedforward = m_feedforward.Calculate(
m_prevSpeeds.frontRight, frontRightSpeedSetpoint);
auto rearRightFeedforward =
m_feedforward.Calculate(m_prevSpeeds.rearRight, rearRightSpeedSetpoint);
auto frontLeftOutput = units::volt_t{m_frontLeftController->Calculate(
m_currentWheelSpeeds().frontLeft.value(),
frontLeftSpeedSetpoint.value())} +
frontLeftFeedforward;
auto rearLeftOutput = units::volt_t{m_rearLeftController->Calculate(
m_currentWheelSpeeds().rearLeft.value(),
rearLeftSpeedSetpoint.value())} +
rearLeftFeedforward;
auto frontRightOutput = units::volt_t{m_frontRightController->Calculate(
m_currentWheelSpeeds().frontRight.value(),
frontRightSpeedSetpoint.value())} +
frontRightFeedforward;
auto rearRightOutput = units::volt_t{m_rearRightController->Calculate(
m_currentWheelSpeeds().rearRight.value(),
rearRightSpeedSetpoint.value())} +
rearRightFeedforward;
m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput,
rearRightOutput);
} else {
m_outputVel(frontLeftSpeedSetpoint, rearLeftSpeedSetpoint,
frontRightSpeedSetpoint, rearRightSpeedSetpoint);
m_prevTime = curTime;
m_prevSpeeds = targetWheelSpeeds;
}
}
void MecanumControllerCommand::End(bool interrupted) {
m_timer.Stop();
}
bool MecanumControllerCommand::IsFinished() {
return m_timer.HasElapsed(m_trajectory.TotalTime());
}

269
wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
View File

@@ -1,269 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <functional>
#include <memory>
#include <frc/Timer.h>
#include <frc/controller/HolonomicDriveController.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/MecanumDriveKinematics.h>
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
#include <frc/trajectory/Trajectory.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/velocity.h>
#include <units/voltage.h>
#include "frc2/command/Command.h"
#include "frc2/command/CommandHelper.h"
#include "frc2/command/Requirements.h"
#pragma once
namespace frc2 {
/**
* A command that uses two PID controllers (PIDController) and a profiled PID
* controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a
* mecanum drive.
*
* <p>The command handles trajectory-following,
* Velocity PID calculations, and feedforwards internally. This
* is intended to be a more-or-less "complete solution" that can be used by
* teams without a great deal of controls expertise.
*
* <p>Advanced teams seeking more flexibility (for example, those who wish to
* use the onboard PID functionality of a "smart" motor controller) may use the
* secondary constructor that omits the PID and feedforward functionality,
* returning only the raw wheel speeds from the PID controllers.
*
* <p>The robot angle controller does not follow the angle given by
* the trajectory but rather goes to the angle given in the final state of the
* trajectory.
*
* This class is provided by the NewCommands VendorDep
*/
class MecanumControllerCommand
: public CommandHelper<Command, MecanumControllerCommand> {
public:
/**
* 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.
*
* @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, frc::PIDController xController,
frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
std::function<frc::Rotation2d()> desiredRotation,
units::meters_per_second_t maxWheelVelocity,
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
frc::PIDController frontLeftController,
frc::PIDController rearLeftController,
frc::PIDController frontRightController,
frc::PIDController rearRightController,
std::function<void(units::volt_t, units::volt_t, units::volt_t,
units::volt_t)>
output,
Requirements 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,
* 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 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, frc::PIDController xController,
frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
units::meters_per_second_t maxWheelVelocity,
std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
frc::PIDController frontLeftController,
frc::PIDController rearLeftController,
frc::PIDController frontRightController,
frc::PIDController rearRightController,
std::function<void(units::volt_t, units::volt_t, units::volt_t,
units::volt_t)>
output,
Requirements 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 nonstationary 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 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, frc::PIDController xController,
frc::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,
Requirements 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 nonstationary 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
* 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 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, frc::PIDController xController,
frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
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,
Requirements requirements = {});
void Initialize() override;
void Execute() override;
void End(bool interrupted) override;
bool IsFinished() override;
private:
frc::Trajectory m_trajectory;
std::function<frc::Pose2d()> m_pose;
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
frc::MecanumDriveKinematics m_kinematics;
frc::HolonomicDriveController m_controller;
std::function<frc::Rotation2d()> m_desiredRotation;
const units::meters_per_second_t m_maxWheelVelocity;
std::unique_ptr<frc::PIDController> m_frontLeftController;
std::unique_ptr<frc::PIDController> m_rearLeftController;
std::unique_ptr<frc::PIDController> m_frontRightController;
std::unique_ptr<frc::PIDController> m_rearRightController;
std::function<frc::MecanumDriveWheelSpeeds()> m_currentWheelSpeeds;
std::function<void(units::meters_per_second_t, units::meters_per_second_t,
units::meters_per_second_t, units::meters_per_second_t)>
m_outputVel;
std::function<void(units::volt_t, units::volt_t, units::volt_t,
units::volt_t)>
m_outputVolts;
bool m_usePID;
frc::Timer m_timer;
frc::MecanumDriveWheelSpeeds m_prevSpeeds;
units::second_t m_prevTime;
};
} // namespace frc2

270
wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
View File

@@ -1,270 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <functional>
#include <memory>
#include <utility>
#include <frc/Timer.h>
#include <frc/controller/HolonomicDriveController.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/trajectory/Trajectory.h>
#include <units/length.h>
#include <units/time.h>
#include <units/voltage.h>
#include "frc2/command/Command.h"
#include "frc2/command/CommandHelper.h"
#include "frc2/command/Requirements.h"
#pragma once
namespace frc2 {
/**
* A command that uses two PID controllers (PIDController) and a profiled PID
* controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a
* swerve drive.
*
* <p>The command handles trajectory-following, Velocity PID calculations, and
* feedforwards internally. This is intended to be a more-or-less "complete
* solution" that can be used by teams without a great deal of controls
* expertise.
*
* <p>Advanced teams seeking more flexibility (for example, those who wish to
* use the onboard PID functionality of a "smart" motor controller) may use the
* secondary constructor that omits the PID and feedforward functionality,
* returning only the raw module states from the position PID controllers.
*
* <p>The robot angle controller does not follow the angle given by
* the trajectory but rather goes to the angle given in the final state of the
* trajectory.
*
* This class is provided by the NewCommands VendorDep
*/
template <size_t NumModules>
class SwerveControllerCommand
: public CommandHelper<Command, SwerveControllerCommand<NumModules>> {
using voltsecondspermeter =
units::compound_unit<units::voltage::volt, units::second,
units::inverse<units::meter>>;
using voltsecondssquaredpermeter =
units::compound_unit<units::voltage::volt, units::squared<units::second>,
units::inverse<units::meter>>;
public:
/**
* 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.
*
* @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,
frc::PIDController xController, frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
Requirements requirements = {})
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_desiredRotation(std::move(desiredRotation)),
m_outputStates(output) {
this->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,
* 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 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,
frc::PIDController xController, frc::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
Requirements requirements = {})
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_outputStates(output) {
this->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.
*
* @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 controller The HolonomicDriveController for the drivetrain.
* @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,
frc::HolonomicDriveController controller,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
Requirements requirements = {})
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(std::move(controller)),
m_desiredRotation(std::move(desiredRotation)),
m_outputStates(output) {
this->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,
* provided by the odometry class.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the drivetrain.
* @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,
frc::HolonomicDriveController controller,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
Requirements requirements = {})
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(std::move(controller)),
m_outputStates(output) {
this->AddRequirements(requirements);
}
void Initialize() override {
if (m_desiredRotation == nullptr) {
m_desiredRotation = [&] {
return m_trajectory.States().back().pose.Rotation();
};
}
m_timer.Restart();
}
void Execute() override {
auto curTime = m_timer.Get();
auto m_desiredState = m_trajectory.Sample(curTime);
auto targetChassisSpeeds =
m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
auto targetModuleStates =
m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
m_outputStates(targetModuleStates);
}
void End(bool interrupted) override { m_timer.Stop(); }
bool IsFinished() override {
return m_timer.HasElapsed(m_trajectory.TotalTime());
}
private:
frc::Trajectory m_trajectory;
std::function<frc::Pose2d()> m_pose;
frc::SwerveDriveKinematics<NumModules> m_kinematics;
frc::HolonomicDriveController m_controller;
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
m_outputStates;
std::function<frc::Rotation2d()> m_desiredRotation;
frc::Timer m_timer;
units::second_t m_prevTime;
frc::Rotation2d m_finalRotation;
};
} // namespace frc2