mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-03 03:01:44 +00:00
[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)
This commit is contained in:
Gold856
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -1,142 +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 org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.hal.HAL;
|
||||
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.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.simulation.SimHooks;
|
||||
import java.util.ArrayList;
|
||||
import org.junit.jupiter.api.AfterEach;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.parallel.ResourceLock;
|
||||
|
||||
class MecanumControllerCommandTest {
|
||||
@BeforeEach
|
||||
void setupAll() {
|
||||
HAL.initialize(500, 0);
|
||||
SimHooks.pauseTiming();
|
||||
}
|
||||
|
||||
@AfterEach
|
||||
void cleanupAll() {
|
||||
SimHooks.resumeTiming();
|
||||
}
|
||||
|
||||
private final Timer m_timer = new Timer();
|
||||
private Rotation2d m_angle = Rotation2d.kZero;
|
||||
|
||||
private double m_frontLeftSpeed;
|
||||
private double m_frontLeftDistance;
|
||||
private double m_rearLeftSpeed;
|
||||
private double m_rearLeftDistance;
|
||||
private double m_frontRightSpeed;
|
||||
private double m_frontRightDistance;
|
||||
private double m_rearRightSpeed;
|
||||
private double m_rearRightDistance;
|
||||
|
||||
private final ProfiledPIDController m_rotController =
|
||||
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
|
||||
|
||||
private static final double kxTolerance = 1 / 12.0;
|
||||
private static final double kyTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = 1 / 12.0;
|
||||
|
||||
private static final double kWheelBase = 0.5;
|
||||
private static final double kTrackwidth = 0.5;
|
||||
|
||||
private final MecanumDriveKinematics m_kinematics =
|
||||
new MecanumDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
|
||||
|
||||
private final MecanumDriveOdometry m_odometry =
|
||||
new MecanumDriveOdometry(
|
||||
m_kinematics, Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero);
|
||||
|
||||
public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
|
||||
this.m_frontLeftSpeed = wheelSpeeds.frontLeft;
|
||||
this.m_rearLeftSpeed = wheelSpeeds.rearLeft;
|
||||
this.m_frontRightSpeed = wheelSpeeds.frontRight;
|
||||
this.m_rearRightSpeed = wheelSpeeds.rearRight;
|
||||
}
|
||||
|
||||
public MecanumDriveWheelPositions getCurrentWheelDistances() {
|
||||
return new MecanumDriveWheelPositions(
|
||||
m_frontLeftDistance, m_frontRightDistance, m_rearLeftDistance, m_rearRightDistance);
|
||||
}
|
||||
|
||||
public Pose2d getRobotPose() {
|
||||
m_odometry.update(m_angle, getCurrentWheelDistances());
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
@Test
|
||||
@ResourceLock("timing")
|
||||
void testReachesReference() {
|
||||
final var subsystem = new Subsystem() {};
|
||||
|
||||
final var waypoints = new ArrayList<Pose2d>();
|
||||
waypoints.add(Pose2d.kZero);
|
||||
waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
|
||||
var config = new TrajectoryConfig(8.8, 0.1);
|
||||
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
final var endState = trajectory.sample(trajectory.getTotalTime());
|
||||
|
||||
final var command =
|
||||
new MecanumControllerCommand(
|
||||
trajectory,
|
||||
this::getRobotPose,
|
||||
m_kinematics,
|
||||
new PIDController(0.6, 0, 0),
|
||||
new PIDController(0.6, 0, 0),
|
||||
m_rotController,
|
||||
8.8,
|
||||
this::setWheelSpeeds,
|
||||
subsystem);
|
||||
|
||||
m_timer.restart();
|
||||
|
||||
command.initialize();
|
||||
while (!command.isFinished()) {
|
||||
command.execute();
|
||||
m_angle = trajectory.sample(m_timer.get()).pose.getRotation();
|
||||
m_frontLeftDistance += m_frontLeftSpeed * 0.005;
|
||||
m_rearLeftDistance += m_rearLeftSpeed * 0.005;
|
||||
m_frontRightDistance += m_frontRightSpeed * 0.005;
|
||||
m_rearRightDistance += m_rearRightSpeed * 0.005;
|
||||
SimHooks.stepTiming(0.005);
|
||||
}
|
||||
m_timer.stop();
|
||||
command.end(true);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance),
|
||||
() -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
endState.pose.getRotation().getRadians(),
|
||||
getRobotPose().getRotation().getRadians(),
|
||||
kAngularTolerance));
|
||||
}
|
||||
}
|
||||
@@ -1,142 +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 org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.hal.HAL;
|
||||
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.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.simulation.SimHooks;
|
||||
import java.util.ArrayList;
|
||||
import org.junit.jupiter.api.AfterEach;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.parallel.ResourceLock;
|
||||
|
||||
class SwerveControllerCommandTest {
|
||||
@BeforeEach
|
||||
void setup() {
|
||||
HAL.initialize(500, 0);
|
||||
SimHooks.pauseTiming();
|
||||
}
|
||||
|
||||
@AfterEach
|
||||
void cleanup() {
|
||||
SimHooks.resumeTiming();
|
||||
}
|
||||
|
||||
private final Timer m_timer = new Timer();
|
||||
private Rotation2d m_angle = Rotation2d.kZero;
|
||||
|
||||
private SwerveModuleState[] m_moduleStates =
|
||||
new SwerveModuleState[] {
|
||||
new SwerveModuleState(0, Rotation2d.kZero),
|
||||
new SwerveModuleState(0, Rotation2d.kZero),
|
||||
new SwerveModuleState(0, Rotation2d.kZero),
|
||||
new SwerveModuleState(0, Rotation2d.kZero)
|
||||
};
|
||||
|
||||
private final SwerveModulePosition[] m_modulePositions =
|
||||
new SwerveModulePosition[] {
|
||||
new SwerveModulePosition(0, Rotation2d.kZero),
|
||||
new SwerveModulePosition(0, Rotation2d.kZero),
|
||||
new SwerveModulePosition(0, Rotation2d.kZero),
|
||||
new SwerveModulePosition(0, Rotation2d.kZero)
|
||||
};
|
||||
|
||||
private final ProfiledPIDController m_rotController =
|
||||
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
|
||||
|
||||
private static final double kxTolerance = 1 / 12.0;
|
||||
private static final double kyTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = 1 / 12.0;
|
||||
|
||||
private static final double kWheelBase = 0.5;
|
||||
private static final double kTrackwidth = 0.5;
|
||||
|
||||
private final SwerveDriveKinematics m_kinematics =
|
||||
new SwerveDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
|
||||
|
||||
private final SwerveDriveOdometry m_odometry =
|
||||
new SwerveDriveOdometry(m_kinematics, Rotation2d.kZero, m_modulePositions, Pose2d.kZero);
|
||||
|
||||
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
|
||||
public void setModuleStates(SwerveModuleState[] moduleStates) {
|
||||
this.m_moduleStates = moduleStates;
|
||||
}
|
||||
|
||||
public Pose2d getRobotPose() {
|
||||
m_odometry.update(m_angle, m_modulePositions);
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
@Test
|
||||
@ResourceLock("timing")
|
||||
void testReachesReference() {
|
||||
final var subsystem = new Subsystem() {};
|
||||
|
||||
final var waypoints = new ArrayList<Pose2d>();
|
||||
waypoints.add(Pose2d.kZero);
|
||||
waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
|
||||
var config = new TrajectoryConfig(8.8, 0.1);
|
||||
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
final var endState = trajectory.sample(trajectory.getTotalTime());
|
||||
|
||||
final var command =
|
||||
new SwerveControllerCommand(
|
||||
trajectory,
|
||||
this::getRobotPose,
|
||||
m_kinematics,
|
||||
new PIDController(0.6, 0, 0),
|
||||
new PIDController(0.6, 0, 0),
|
||||
m_rotController,
|
||||
this::setModuleStates,
|
||||
subsystem);
|
||||
|
||||
m_timer.restart();
|
||||
|
||||
command.initialize();
|
||||
while (!command.isFinished()) {
|
||||
command.execute();
|
||||
m_angle = trajectory.sample(m_timer.get()).pose.getRotation();
|
||||
|
||||
for (int i = 0; i < m_modulePositions.length; i++) {
|
||||
m_modulePositions[i].distance += m_moduleStates[i].speed * 0.005;
|
||||
m_modulePositions[i].angle = m_moduleStates[i].angle;
|
||||
}
|
||||
|
||||
SimHooks.stepTiming(0.005);
|
||||
}
|
||||
m_timer.stop();
|
||||
command.end(true);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance),
|
||||
() -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
endState.pose.getRotation().getRadians(),
|
||||
getRobotPose().getRotation().getRadians(),
|
||||
kAngularTolerance));
|
||||
}
|
||||
}
|
||||
@@ -1,135 +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 <frc2/command/Subsystem.h>
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/kinematics/MecanumDriveOdometry.h>
|
||||
#include <frc/simulation/SimHooks.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "CommandTestBase.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
class MecanumControllerCommandTest : public ::testing::Test {
|
||||
using radians_per_second_squared_t =
|
||||
units::compound_unit<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
protected:
|
||||
frc::Timer m_timer;
|
||||
frc::Rotation2d m_angle{0_rad};
|
||||
|
||||
units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
|
||||
units::meter_t m_frontLeftDistance = 0.0_m;
|
||||
units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
|
||||
units::meter_t m_rearLeftDistance = 0.0_m;
|
||||
units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
|
||||
units::meter_t m_frontRightDistance = 0.0_m;
|
||||
units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
|
||||
units::meter_t m_rearRightDistance = 0.0_m;
|
||||
|
||||
frc::ProfiledPIDController<units::radians> m_rotController{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radians>::Constraints{
|
||||
9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
|
||||
|
||||
static constexpr units::meter_t kxTolerance{1 / 12.0};
|
||||
static constexpr units::meter_t kyTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{1 / 12.0};
|
||||
|
||||
static constexpr units::meter_t kWheelBase{0.5};
|
||||
static constexpr units::meter_t kTrackwidth{0.5};
|
||||
|
||||
frc::MecanumDriveKinematics m_kinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
|
||||
getCurrentWheelDistances(),
|
||||
frc::Pose2d{0_m, 0_m, 0_rad}};
|
||||
|
||||
void SetUp() override { frc::sim::PauseTiming(); }
|
||||
|
||||
void TearDown() override { frc::sim::ResumeTiming(); }
|
||||
|
||||
frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
|
||||
return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
|
||||
m_rearLeftSpeed, m_rearRightSpeed};
|
||||
}
|
||||
|
||||
frc::MecanumDriveWheelPositions getCurrentWheelDistances() {
|
||||
return frc::MecanumDriveWheelPositions{
|
||||
m_frontLeftDistance,
|
||||
m_rearLeftDistance,
|
||||
m_frontRightDistance,
|
||||
m_rearRightDistance,
|
||||
};
|
||||
}
|
||||
|
||||
frc::Pose2d getRobotPose() {
|
||||
m_odometry.Update(m_angle, getCurrentWheelDistances());
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(MecanumControllerCommandTest, ReachesReference) {
|
||||
frc2::TestSubsystem subsystem;
|
||||
|
||||
auto waypoints =
|
||||
std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.8_mps, 0.1_mps_sq});
|
||||
|
||||
auto endState = trajectory.Sample(trajectory.TotalTime());
|
||||
|
||||
auto command = frc2::MecanumControllerCommand(
|
||||
trajectory, [&]() { return getRobotPose(); }, m_kinematics,
|
||||
|
||||
frc::PIDController(0.6, 0, 0), frc::PIDController(0.6, 0, 0),
|
||||
m_rotController, 8.8_mps,
|
||||
[&](units::meters_per_second_t frontLeft,
|
||||
units::meters_per_second_t rearLeft,
|
||||
units::meters_per_second_t frontRight,
|
||||
units::meters_per_second_t rearRight) {
|
||||
m_frontLeftSpeed = frontLeft;
|
||||
m_rearLeftSpeed = rearLeft;
|
||||
m_frontRightSpeed = frontRight;
|
||||
m_rearRightSpeed = rearRight;
|
||||
},
|
||||
{&subsystem});
|
||||
|
||||
m_timer.Restart();
|
||||
|
||||
command.Initialize();
|
||||
while (!command.IsFinished()) {
|
||||
command.Execute();
|
||||
m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
|
||||
m_frontLeftDistance += m_frontLeftSpeed * 5_ms;
|
||||
m_rearLeftDistance += m_rearLeftSpeed * 5_ms;
|
||||
m_frontRightDistance += m_frontRightSpeed * 5_ms;
|
||||
m_rearRightDistance += m_rearRightSpeed * 5_ms;
|
||||
frc::sim::StepTiming(5_ms);
|
||||
}
|
||||
m_timer.Stop();
|
||||
command.End(false);
|
||||
|
||||
EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
|
||||
getRobotPose().Rotation().Radians(), kAngularTolerance);
|
||||
}
|
||||
@@ -1,113 +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/Subsystem.h>
|
||||
#include <frc2/command/SwerveControllerCommand.h>
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveDriveOdometry.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/simulation/SimHooks.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "CommandTestBase.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
class SwerveControllerCommandTest : public ::testing::Test {
|
||||
using radians_per_second_squared_t =
|
||||
units::compound_unit<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
protected:
|
||||
frc::Timer m_timer;
|
||||
frc::Rotation2d m_angle{0_rad};
|
||||
|
||||
wpi::array<frc::SwerveModuleState, 4> m_moduleStates{
|
||||
frc::SwerveModuleState{}, frc::SwerveModuleState{},
|
||||
frc::SwerveModuleState{}, frc::SwerveModuleState{}};
|
||||
|
||||
wpi::array<frc::SwerveModulePosition, 4> m_modulePositions{
|
||||
frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
|
||||
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}};
|
||||
|
||||
frc::ProfiledPIDController<units::radians> m_rotController{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radians>::Constraints{
|
||||
9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
|
||||
|
||||
static constexpr units::meter_t kxTolerance{1 / 12.0};
|
||||
static constexpr units::meter_t kyTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{1 / 12.0};
|
||||
|
||||
static constexpr units::meter_t kWheelBase{0.5};
|
||||
static constexpr units::meter_t kTrackwidth{0.5};
|
||||
|
||||
frc::SwerveDriveKinematics<4> m_kinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, m_modulePositions,
|
||||
frc::Pose2d{0_m, 0_m, 0_rad}};
|
||||
|
||||
void SetUp() override { frc::sim::PauseTiming(); }
|
||||
|
||||
void TearDown() override { frc::sim::ResumeTiming(); }
|
||||
|
||||
frc::Pose2d getRobotPose() {
|
||||
m_odometry.Update(m_angle, m_modulePositions);
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(SwerveControllerCommandTest, ReachesReference) {
|
||||
frc2::TestSubsystem subsystem;
|
||||
|
||||
auto waypoints =
|
||||
std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.8_mps, 0.1_mps_sq});
|
||||
|
||||
auto endState = trajectory.Sample(trajectory.TotalTime());
|
||||
|
||||
auto command = frc2::SwerveControllerCommand<4>(
|
||||
trajectory, [&]() { return getRobotPose(); }, m_kinematics,
|
||||
|
||||
frc::PIDController(0.6, 0, 0), frc::PIDController(0.6, 0, 0),
|
||||
m_rotController,
|
||||
[&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem});
|
||||
|
||||
m_timer.Restart();
|
||||
|
||||
command.Initialize();
|
||||
while (!command.IsFinished()) {
|
||||
command.Execute();
|
||||
m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
|
||||
|
||||
for (size_t i = 0; i < m_modulePositions.size(); i++) {
|
||||
m_modulePositions[i].distance += m_moduleStates[i].speed * 5_ms;
|
||||
m_modulePositions[i].angle = m_moduleStates[i].angle;
|
||||
}
|
||||
|
||||
frc::sim::StepTiming(5_ms);
|
||||
}
|
||||
m_timer.Stop();
|
||||
command.End(false);
|
||||
|
||||
EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
|
||||
getRobotPose().Rotation().Radians(), kAngularTolerance);
|
||||
}
|
||||
Reference in New Issue
Block a user