mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +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);
|
||||
}
|
||||
@@ -30,7 +30,6 @@ EXAMPLE_FOLDERS = [
|
||||
"I2CCommunication",
|
||||
"IntermediateVision",
|
||||
"MecanumBot",
|
||||
"MecanumControllerCommand",
|
||||
"MecanumDrive",
|
||||
"MecanumDrivePoseEstimator",
|
||||
"Mechanism2d",
|
||||
@@ -47,7 +46,6 @@ EXAMPLE_FOLDERS = [
|
||||
"StateSpaceFlywheel",
|
||||
"StateSpaceFlywheelSysId",
|
||||
"SwerveBot",
|
||||
"SwerveControllerCommand",
|
||||
"SwerveDrivePoseEstimator",
|
||||
"SysIdRoutine",
|
||||
"TankDrive",
|
||||
@@ -99,8 +97,6 @@ TESTS_FOLDERS = [
|
||||
"DigitalCommunication",
|
||||
"ElevatorSimulation",
|
||||
"I2CCommunication",
|
||||
"MecanumControllerCommand",
|
||||
"PotentiometerPID",
|
||||
"SwerveControllerCommand",
|
||||
"UnitTest",
|
||||
]
|
||||
|
||||
@@ -1,22 +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 "Constants.h"
|
||||
|
||||
namespace DriveConstants {
|
||||
|
||||
const frc::MecanumDriveKinematics kDriveKinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
|
||||
const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
|
||||
|
||||
} // namespace AutoConstants
|
||||
@@ -1,72 +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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
void Robot::RobotPeriodic() {
|
||||
frc2::CommandScheduler::GetInstance().Run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode. You
|
||||
* can use it to reset any subsystem information you want to clear when the
|
||||
* robot is disabled.
|
||||
*/
|
||||
void Robot::DisabledInit() {}
|
||||
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link
|
||||
* RobotContainer} class.
|
||||
*/
|
||||
void Robot::AutonomousInit() {
|
||||
m_autonomousCommand = m_container.GetAutonomousCommand();
|
||||
|
||||
if (m_autonomousCommand) {
|
||||
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,121 +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 "RobotContainer.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/MecanumControllerCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.Drive(-m_driverController.GetLeftY(),
|
||||
-m_driverController.GetRightX(),
|
||||
-m_driverController.GetLeftX(), false);
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.OnTrue(&m_driveHalfSpeed)
|
||||
.OnFalse(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
// Set up config for trajectory
|
||||
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
|
||||
AutoConstants::kMaxAcceleration);
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
config.SetKinematics(DriveConstants::kDriveKinematics);
|
||||
|
||||
// An example trajectory to follow. All units in meters.
|
||||
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
frc::Pose2d{0_m, 0_m, 0_deg},
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
{frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
frc::Pose2d{3_m, 0_m, 0_deg},
|
||||
// Pass the config
|
||||
config);
|
||||
|
||||
frc2::CommandPtr mecanumControllerCommand =
|
||||
frc2::MecanumControllerCommand(
|
||||
exampleTrajectory, [this]() { return m_drive.GetPose(); },
|
||||
|
||||
frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
|
||||
DriveConstants::kDriveKinematics,
|
||||
|
||||
frc::PIDController{AutoConstants::kPXController, 0, 0},
|
||||
frc::PIDController{AutoConstants::kPYController, 0, 0},
|
||||
frc::ProfiledPIDController<units::radians>(
|
||||
AutoConstants::kPThetaController, 0, 0,
|
||||
AutoConstants::kThetaControllerConstraints),
|
||||
|
||||
AutoConstants::kMaxSpeed,
|
||||
|
||||
[this]() {
|
||||
return frc::MecanumDriveWheelSpeeds{
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetFrontLeftEncoder().GetRate()},
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetFrontRightEncoder().GetRate()},
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetRearLeftEncoder().GetRate()},
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetRearRightEncoder().GetRate()}};
|
||||
},
|
||||
|
||||
frc::PIDController{DriveConstants::kPFrontLeftVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPRearLeftVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPFrontRightVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPRearRightVel, 0, 0},
|
||||
|
||||
[this](units::volt_t frontLeft, units::volt_t rearLeft,
|
||||
units::volt_t frontRight, units::volt_t rearRight) {
|
||||
m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight,
|
||||
rearRight);
|
||||
},
|
||||
|
||||
{&m_drive})
|
||||
.ToPtr();
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return frc2::cmd::Sequence(
|
||||
frc2::InstantCommand(
|
||||
[this, initialPose = exampleTrajectory.InitialPose()]() {
|
||||
m_drive.ResetOdometry(initialPose);
|
||||
},
|
||||
{})
|
||||
.ToPtr(),
|
||||
std::move(mecanumControllerCommand),
|
||||
frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {})
|
||||
.ToPtr());
|
||||
}
|
||||
@@ -1,136 +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 "subsystems/DriveSubsystem.h"
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_frontLeft{kFrontLeftMotorPort},
|
||||
m_rearLeft{kRearLeftMotorPort},
|
||||
m_frontRight{kFrontRightMotorPort},
|
||||
m_rearRight{kRearRightMotorPort},
|
||||
|
||||
m_frontLeftEncoder{kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
|
||||
kFrontLeftEncoderReversed},
|
||||
m_rearLeftEncoder{kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
|
||||
kRearLeftEncoderReversed},
|
||||
m_frontRightEncoder{kFrontRightEncoderPorts[0],
|
||||
kFrontRightEncoderPorts[1],
|
||||
kFrontRightEncoderReversed},
|
||||
m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
|
||||
kRearRightEncoderReversed},
|
||||
|
||||
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(),
|
||||
getCurrentWheelDistances(), frc::Pose2d{}} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_frontLeft);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_rearLeft);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_frontRight);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_rearRight);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_frontRight.SetInverted(true);
|
||||
m_rearRight.SetInverted(true);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
m_odometry.Update(m_gyro.GetRotation2d(), getCurrentWheelDistances());
|
||||
}
|
||||
|
||||
void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
|
||||
bool fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
m_drive.DriveCartesian(xSpeed, ySpeed, rot, m_gyro.GetRotation2d());
|
||||
} else {
|
||||
m_drive.DriveCartesian(xSpeed, ySpeed, rot);
|
||||
}
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMotorControllersVolts(units::volt_t frontLeftPower,
|
||||
units::volt_t rearLeftPower,
|
||||
units::volt_t frontRightPower,
|
||||
units::volt_t rearRightPower) {
|
||||
m_frontLeft.SetVoltage(frontLeftPower);
|
||||
m_rearLeft.SetVoltage(rearLeftPower);
|
||||
m_frontRight.SetVoltage(frontRightPower);
|
||||
m_rearRight.SetVoltage(rearRightPower);
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_frontLeftEncoder.Reset();
|
||||
m_rearLeftEncoder.Reset();
|
||||
m_frontRightEncoder.Reset();
|
||||
m_rearRightEncoder.Reset();
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetFrontLeftEncoder() {
|
||||
return m_frontLeftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRearLeftEncoder() {
|
||||
return m_rearLeftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetFrontRightEncoder() {
|
||||
return m_frontRightEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRearRightEncoder() {
|
||||
return m_rearRightEncoder;
|
||||
}
|
||||
|
||||
frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
|
||||
return (frc::MecanumDriveWheelSpeeds{
|
||||
units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_rearLeftEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_rearRightEncoder.GetRate()}});
|
||||
}
|
||||
|
||||
frc::MecanumDriveWheelPositions DriveSubsystem::getCurrentWheelDistances() {
|
||||
return (frc::MecanumDriveWheelPositions{
|
||||
units::meter_t{m_frontLeftEncoder.GetDistance()},
|
||||
units::meter_t{m_rearLeftEncoder.GetDistance()},
|
||||
units::meter_t{m_frontRightEncoder.GetDistance()},
|
||||
units::meter_t{m_rearRightEncoder.GetDistance()}});
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMaxOutput(double maxOutput) {
|
||||
m_drive.SetMaxOutput(maxOutput);
|
||||
}
|
||||
|
||||
units::degree_t DriveSubsystem::GetHeading() const {
|
||||
return m_gyro.GetRotation2d().Degrees();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ZeroHeading() {
|
||||
m_gyro.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return -m_gyro.GetRate();
|
||||
}
|
||||
|
||||
frc::Pose2d DriveSubsystem::GetPose() {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(), getCurrentWheelDistances(),
|
||||
pose);
|
||||
}
|
||||
@@ -1,91 +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 <numbers>
|
||||
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
#include <units/time.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* The Constants header provides a convenient place for teams to hold robot-wide
|
||||
* numerical or bool constants. This should not be used for any other purpose.
|
||||
*
|
||||
* It is generally a good idea to place constants into subsystem- or
|
||||
* command-specific namespaces within this header, which can then be used where
|
||||
* they are needed.
|
||||
*/
|
||||
|
||||
namespace DriveConstants {
|
||||
inline constexpr int kFrontLeftMotorPort = 0;
|
||||
inline constexpr int kRearLeftMotorPort = 1;
|
||||
inline constexpr int kFrontRightMotorPort = 2;
|
||||
inline constexpr int kRearRightMotorPort = 3;
|
||||
|
||||
inline constexpr int kFrontLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRearLeftEncoderPorts[]{2, 3};
|
||||
inline constexpr int kFrontRightEncoderPorts[]{4, 5};
|
||||
inline constexpr int kRearRightEncoderPorts[]{6, 7};
|
||||
|
||||
inline constexpr bool kFrontLeftEncoderReversed = false;
|
||||
inline constexpr bool kRearLeftEncoderReversed = true;
|
||||
inline constexpr bool kFrontRightEncoderReversed = false;
|
||||
inline constexpr bool kRearRightEncoderReversed = true;
|
||||
|
||||
inline constexpr auto kTrackwidth =
|
||||
0.5_m; // Distance between centers of right and left wheels on robot
|
||||
inline constexpr auto kWheelBase =
|
||||
0.7_m; // Distance between centers of front and back wheels on robot
|
||||
extern const frc::MecanumDriveKinematics kDriveKinematics;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr auto kWheelDiameter = 0.15_m;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter.value() * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or
|
||||
// theoretically for *your* robot's drive. The SysId tool provides a convenient
|
||||
// method for obtaining these values for your robot.
|
||||
inline constexpr auto ks = 1_V;
|
||||
inline constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
|
||||
inline constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
inline constexpr double kPFrontLeftVel = 0.5;
|
||||
inline constexpr double kPRearLeftVel = 0.5;
|
||||
inline constexpr double kPFrontRightVel = 0.5;
|
||||
inline constexpr double kPRearRightVel = 0.5;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kMaxSpeed = 3_mps;
|
||||
inline constexpr auto kMaxAcceleration = 3_mps_sq;
|
||||
inline constexpr auto kMaxAngularSpeed = 3_rad_per_s;
|
||||
inline constexpr auto kMaxAngularAcceleration = 3_rad_per_s_sq;
|
||||
|
||||
inline constexpr double kPXController = 0.5;
|
||||
inline constexpr double kPYController = 0.5;
|
||||
inline constexpr double kPThetaController = 0.5;
|
||||
|
||||
extern const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints;
|
||||
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,32 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,48 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/ParallelRaceGroup.h>
|
||||
#include <frc2/command/RunCommand.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
class RobotContainer {
|
||||
public:
|
||||
RobotContainer();
|
||||
|
||||
frc2::CommandPtr GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
|
||||
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
|
||||
{}};
|
||||
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
|
||||
{}};
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,172 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/AnalogGyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/MecanumDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveOdometry.h>
|
||||
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1]
|
||||
* and the linear speeds have no effect on the angular speed.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction
|
||||
* (forward/backwards).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to
|
||||
* the field.
|
||||
*/
|
||||
void Drive(double xSpeed, double ySpeed, double rot, bool fieldRelative);
|
||||
|
||||
/**
|
||||
* Resets the drive encoders to currently read a position of 0.
|
||||
*/
|
||||
void ResetEncoders();
|
||||
|
||||
/**
|
||||
* Gets the front left drive encoder.
|
||||
*
|
||||
* @return the front left drive encoder
|
||||
*/
|
||||
frc::Encoder& GetFrontLeftEncoder();
|
||||
|
||||
/**
|
||||
* Gets the rear left drive encoder.
|
||||
*
|
||||
* @return the rear left drive encoder
|
||||
*/
|
||||
frc::Encoder& GetRearLeftEncoder();
|
||||
|
||||
/**
|
||||
* Gets the front right drive encoder.
|
||||
*
|
||||
* @return the front right drive encoder
|
||||
*/
|
||||
frc::Encoder& GetFrontRightEncoder();
|
||||
|
||||
/**
|
||||
* Gets the rear right drive encoder.
|
||||
*
|
||||
* @return the rear right drive encoder
|
||||
*/
|
||||
frc::Encoder& GetRearRightEncoder();
|
||||
|
||||
/**
|
||||
* Gets the wheel speeds.
|
||||
*
|
||||
* @return the current wheel speeds.
|
||||
*/
|
||||
frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
|
||||
|
||||
/**
|
||||
* Gets the distances travelled by each wheel.
|
||||
*
|
||||
* @return the distances travelled by each wheel.
|
||||
*/
|
||||
frc::MecanumDriveWheelPositions getCurrentWheelDistances();
|
||||
|
||||
/**
|
||||
* Sets the drive MotorControllers to a desired voltage.
|
||||
*/
|
||||
void SetMotorControllersVolts(units::volt_t frontLeftPower,
|
||||
units::volt_t rearLeftPower,
|
||||
units::volt_t frontRightPower,
|
||||
units::volt_t rearRightPower);
|
||||
|
||||
/**
|
||||
* Sets the max output of the drive. Useful for scaling the drive to drive
|
||||
* more slowly.
|
||||
*
|
||||
* @param maxOutput the maximum output to which the drive will be constrained
|
||||
*/
|
||||
void SetMaxOutput(double maxOutput);
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
units::degree_t GetHeading() const;
|
||||
|
||||
/**
|
||||
* Zeroes the heading of the robot.
|
||||
*/
|
||||
void ZeroHeading();
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
double GetTurnRate();
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
frc::Pose2d GetPose();
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
void ResetOdometry(frc::Pose2d pose);
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_frontLeft;
|
||||
frc::PWMSparkMax m_rearLeft;
|
||||
frc::PWMSparkMax m_frontRight;
|
||||
frc::PWMSparkMax m_rearRight;
|
||||
|
||||
// The robot's drive
|
||||
frc::MecanumDrive m_drive{[&](double output) { m_frontLeft.Set(output); },
|
||||
[&](double output) { m_rearLeft.Set(output); },
|
||||
[&](double output) { m_frontRight.Set(output); },
|
||||
[&](double output) { m_rearRight.Set(output); }};
|
||||
|
||||
// The front-left-side drive encoder
|
||||
frc::Encoder m_frontLeftEncoder;
|
||||
|
||||
// The rear-left-side drive encoder
|
||||
frc::Encoder m_rearLeftEncoder;
|
||||
|
||||
// The front-right--side drive encoder
|
||||
frc::Encoder m_frontRightEncoder;
|
||||
|
||||
// The rear-right-side drive encoder
|
||||
frc::Encoder m_rearRightEncoder;
|
||||
|
||||
// The gyro sensor
|
||||
frc::AnalogGyro m_gyro{0};
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
frc::MecanumDriveOdometry m_odometry;
|
||||
};
|
||||
@@ -1,12 +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 "Constants.h"
|
||||
|
||||
namespace AutoConstants {
|
||||
|
||||
const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
|
||||
|
||||
} // namespace AutoConstants
|
||||
@@ -1,72 +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 "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
void Robot::RobotPeriodic() {
|
||||
frc2::CommandScheduler::GetInstance().Run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode. You
|
||||
* can use it to reset any subsystem information you want to clear when the
|
||||
* robot is disabled.
|
||||
*/
|
||||
void Robot::DisabledInit() {}
|
||||
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link
|
||||
* RobotContainer} class.
|
||||
*/
|
||||
void Robot::AutonomousInit() {
|
||||
m_autonomousCommand = m_container.GetAutonomousCommand();
|
||||
|
||||
if (m_autonomousCommand) {
|
||||
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,104 +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 "RobotContainer.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/SwerveControllerCommand.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/velocity.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
// The left stick controls translation of the robot.
|
||||
// Turning is controlled by the X axis of the right stick.
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.Drive(
|
||||
// Multiply by max speed to map the joystick unitless inputs to
|
||||
// actual units. This will map the [-1, 1] to [max speed backwards,
|
||||
// max speed forwards], converting them to actual units.
|
||||
m_driverController.GetLeftY() * AutoConstants::kMaxSpeed,
|
||||
m_driverController.GetLeftX() * AutoConstants::kMaxSpeed,
|
||||
m_driverController.GetRightX() * AutoConstants::kMaxAngularSpeed,
|
||||
false);
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
// Set up config for trajectory
|
||||
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
|
||||
AutoConstants::kMaxAcceleration);
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
config.SetKinematics(m_drive.kDriveKinematics);
|
||||
|
||||
// An example trajectory to follow. All units in meters.
|
||||
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
frc::Pose2d{0_m, 0_m, 0_deg},
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
{frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
frc::Pose2d{3_m, 0_m, 0_deg},
|
||||
// Pass the config
|
||||
config);
|
||||
|
||||
frc::ProfiledPIDController<units::radians> thetaController{
|
||||
AutoConstants::kPThetaController, 0, 0,
|
||||
AutoConstants::kThetaControllerConstraints};
|
||||
|
||||
thetaController.EnableContinuousInput(units::radian_t{-std::numbers::pi},
|
||||
units::radian_t{std::numbers::pi});
|
||||
|
||||
frc2::CommandPtr swerveControllerCommand =
|
||||
frc2::SwerveControllerCommand<4>(
|
||||
exampleTrajectory, [this]() { return m_drive.GetPose(); },
|
||||
|
||||
m_drive.kDriveKinematics,
|
||||
|
||||
frc::PIDController{AutoConstants::kPXController, 0, 0},
|
||||
frc::PIDController{AutoConstants::kPYController, 0, 0},
|
||||
thetaController,
|
||||
|
||||
[this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
|
||||
|
||||
{&m_drive})
|
||||
.ToPtr();
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return frc2::cmd::Sequence(
|
||||
frc2::InstantCommand(
|
||||
[this, initialPose = exampleTrajectory.InitialPose()]() {
|
||||
m_drive.ResetOdometry(initialPose);
|
||||
},
|
||||
{})
|
||||
.ToPtr(),
|
||||
std::move(swerveControllerCommand),
|
||||
frc2::InstantCommand(
|
||||
[this] { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {})
|
||||
.ToPtr());
|
||||
}
|
||||
@@ -1,111 +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 "subsystems/DriveSubsystem.h"
|
||||
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/velocity.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_frontLeft{kFrontLeftDriveMotorPort,
|
||||
kFrontLeftTurningMotorPort,
|
||||
kFrontLeftDriveEncoderPorts,
|
||||
kFrontLeftTurningEncoderPorts,
|
||||
kFrontLeftDriveEncoderReversed,
|
||||
kFrontLeftTurningEncoderReversed},
|
||||
|
||||
m_rearLeft{
|
||||
kRearLeftDriveMotorPort, kRearLeftTurningMotorPort,
|
||||
kRearLeftDriveEncoderPorts, kRearLeftTurningEncoderPorts,
|
||||
kRearLeftDriveEncoderReversed, kRearLeftTurningEncoderReversed},
|
||||
|
||||
m_frontRight{
|
||||
kFrontRightDriveMotorPort, kFrontRightTurningMotorPort,
|
||||
kFrontRightDriveEncoderPorts, kFrontRightTurningEncoderPorts,
|
||||
kFrontRightDriveEncoderReversed, kFrontRightTurningEncoderReversed},
|
||||
|
||||
m_rearRight{
|
||||
kRearRightDriveMotorPort, kRearRightTurningMotorPort,
|
||||
kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts,
|
||||
kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
|
||||
|
||||
m_odometry{kDriveKinematics,
|
||||
m_gyro.GetRotation2d(),
|
||||
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
|
||||
m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
|
||||
frc::Pose2d{}} {}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
m_odometry.Update(m_gyro.GetRotation2d(),
|
||||
{m_frontLeft.GetPosition(), m_rearLeft.GetPosition(),
|
||||
m_frontRight.GetPosition(), m_rearRight.GetPosition()});
|
||||
}
|
||||
|
||||
void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
|
||||
units::meters_per_second_t ySpeed,
|
||||
units::radians_per_second_t rot, bool fieldRelative,
|
||||
units::second_t period) {
|
||||
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
|
||||
if (fieldRelative) {
|
||||
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
|
||||
}
|
||||
chassisSpeeds = chassisSpeeds.Discretize(period);
|
||||
|
||||
auto states = kDriveKinematics.ToSwerveModuleStates(chassisSpeeds);
|
||||
kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);
|
||||
|
||||
auto [fl, fr, bl, br] = states;
|
||||
m_frontLeft.SetDesiredState(fl);
|
||||
m_frontRight.SetDesiredState(fr);
|
||||
m_rearLeft.SetDesiredState(bl);
|
||||
m_rearRight.SetDesiredState(br);
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetModuleStates(
|
||||
wpi::array<frc::SwerveModuleState, 4> desiredStates) {
|
||||
kDriveKinematics.DesaturateWheelSpeeds(&desiredStates,
|
||||
AutoConstants::kMaxSpeed);
|
||||
m_frontLeft.SetDesiredState(desiredStates[0]);
|
||||
m_frontRight.SetDesiredState(desiredStates[1]);
|
||||
m_rearLeft.SetDesiredState(desiredStates[2]);
|
||||
m_rearRight.SetDesiredState(desiredStates[3]);
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_frontLeft.ResetEncoders();
|
||||
m_rearLeft.ResetEncoders();
|
||||
m_frontRight.ResetEncoders();
|
||||
m_rearRight.ResetEncoders();
|
||||
}
|
||||
|
||||
units::degree_t DriveSubsystem::GetHeading() const {
|
||||
return m_gyro.GetRotation2d().Degrees();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ZeroHeading() {
|
||||
m_gyro.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return -m_gyro.GetRate();
|
||||
}
|
||||
|
||||
frc::Pose2d DriveSubsystem::GetPose() {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
m_odometry.ResetPosition(
|
||||
GetHeading(),
|
||||
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
|
||||
m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
|
||||
pose);
|
||||
}
|
||||
@@ -1,85 +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 "subsystems/SwerveModule.h"
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel,
|
||||
const int driveEncoderPorts[2],
|
||||
const int turningEncoderPorts[2],
|
||||
bool driveEncoderReversed,
|
||||
bool turningEncoderReversed)
|
||||
: m_driveMotor(driveMotorChannel),
|
||||
m_turningMotor(turningMotorChannel),
|
||||
m_driveEncoder(driveEncoderPorts[0], driveEncoderPorts[1]),
|
||||
m_turningEncoder(turningEncoderPorts[0], turningEncoderPorts[1]) {
|
||||
// Set the distance per pulse for the drive encoder. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_driveEncoder.SetDistancePerPulse(
|
||||
ModuleConstants::kDriveEncoderDistancePerPulse);
|
||||
|
||||
// Set whether drive encoder should be reversed or not
|
||||
m_driveEncoder.SetReverseDirection(driveEncoderReversed);
|
||||
|
||||
// Set the distance (in this case, angle) per pulse for the turning encoder.
|
||||
// This is the the angle through an entire rotation (2 * std::numbers::pi)
|
||||
// divided by the encoder resolution.
|
||||
m_turningEncoder.SetDistancePerPulse(
|
||||
ModuleConstants::kTurningEncoderDistancePerPulse);
|
||||
|
||||
// Set whether turning encoder should be reversed or not
|
||||
m_turningEncoder.SetReverseDirection(turningEncoderReversed);
|
||||
|
||||
// Limit the PID Controller's input range between -pi and pi and set the input
|
||||
// to be continuous.
|
||||
m_turningPIDController.EnableContinuousInput(
|
||||
units::radian_t{-std::numbers::pi}, units::radian_t{std::numbers::pi});
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() {
|
||||
return {units::meters_per_second_t{m_driveEncoder.GetRate()},
|
||||
units::radian_t{m_turningEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() {
|
||||
return {units::meter_t{m_driveEncoder.GetDistance()},
|
||||
units::radian_t{m_turningEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
|
||||
frc::Rotation2d encoderRotation{
|
||||
units::radian_t{m_turningEncoder.GetDistance()}};
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees
|
||||
referenceState.Optimize(encoderRotation);
|
||||
|
||||
// Scale speed by cosine of angle error. This scales down movement
|
||||
// perpendicular to the desired direction of travel that can occur when
|
||||
// modules change directions. This results in smoother driving.
|
||||
referenceState.CosineScale(encoderRotation);
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
const auto driveOutput = m_drivePIDController.Calculate(
|
||||
m_driveEncoder.GetRate(), referenceState.speed.value());
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
auto turnOutput = m_turningPIDController.Calculate(
|
||||
units::radian_t{m_turningEncoder.GetDistance()},
|
||||
referenceState.angle.Radians());
|
||||
|
||||
// Set the motor outputs.
|
||||
m_driveMotor.Set(driveOutput);
|
||||
m_turningMotor.Set(turnOutput);
|
||||
}
|
||||
|
||||
void SwerveModule::ResetEncoders() {
|
||||
m_driveEncoder.Reset();
|
||||
m_turningEncoder.Reset();
|
||||
}
|
||||
@@ -1,116 +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 <numbers>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
#include <units/time.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* The Constants header provides a convenient place for teams to hold robot-wide
|
||||
* numerical or bool constants. This should not be used for any other purpose.
|
||||
*
|
||||
* It is generally a good idea to place constants into subsystem- or
|
||||
* command-specific namespaces within this header, which can then be used where
|
||||
* they are needed.
|
||||
*/
|
||||
|
||||
namespace DriveConstants {
|
||||
inline constexpr int kFrontLeftDriveMotorPort = 0;
|
||||
inline constexpr int kRearLeftDriveMotorPort = 2;
|
||||
inline constexpr int kFrontRightDriveMotorPort = 4;
|
||||
inline constexpr int kRearRightDriveMotorPort = 6;
|
||||
|
||||
inline constexpr int kFrontLeftTurningMotorPort = 1;
|
||||
inline constexpr int kRearLeftTurningMotorPort = 3;
|
||||
inline constexpr int kFrontRightTurningMotorPort = 5;
|
||||
inline constexpr int kRearRightTurningMotorPort = 7;
|
||||
|
||||
inline constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
|
||||
inline constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
|
||||
inline constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
|
||||
inline constexpr int kRearRightTurningEncoderPorts[2]{6, 7};
|
||||
|
||||
inline constexpr bool kFrontLeftTurningEncoderReversed = false;
|
||||
inline constexpr bool kRearLeftTurningEncoderReversed = true;
|
||||
inline constexpr bool kFrontRightTurningEncoderReversed = false;
|
||||
inline constexpr bool kRearRightTurningEncoderReversed = true;
|
||||
|
||||
inline constexpr int kFrontLeftDriveEncoderPorts[2]{8, 9};
|
||||
inline constexpr int kRearLeftDriveEncoderPorts[2]{10, 11};
|
||||
inline constexpr int kFrontRightDriveEncoderPorts[2]{12, 13};
|
||||
inline constexpr int kRearRightDriveEncoderPorts[2]{14, 15};
|
||||
|
||||
inline constexpr bool kFrontLeftDriveEncoderReversed = false;
|
||||
inline constexpr bool kRearLeftDriveEncoderReversed = true;
|
||||
inline constexpr bool kFrontRightDriveEncoderReversed = false;
|
||||
inline constexpr bool kRearRightDriveEncoderReversed = true;
|
||||
|
||||
// If you call DriveSubsystem::Drive with a different period make sure to update
|
||||
// this.
|
||||
inline constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or
|
||||
// theoretically for *your* robot's drive. The SysId tool provides a convenient
|
||||
// method for obtaining these values for your robot.
|
||||
inline constexpr auto ks = 1_V;
|
||||
inline constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
|
||||
inline constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
inline constexpr double kPFrontLeftVel = 0.5;
|
||||
inline constexpr double kPRearLeftVel = 0.5;
|
||||
inline constexpr double kPFrontRightVel = 0.5;
|
||||
inline constexpr double kPRearRightVel = 0.5;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace ModuleConstants {
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr auto kWheelDiameter = 0.15_m;
|
||||
inline constexpr double kDriveEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter.value() * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
|
||||
inline constexpr double kTurningEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(std::numbers::pi * 2) / static_cast<double>(kEncoderCPR);
|
||||
|
||||
inline constexpr double kPModuleTurningController = 1;
|
||||
inline constexpr double kPModuleDriveController = 1;
|
||||
} // namespace ModuleConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kMaxSpeed = 3_mps;
|
||||
inline constexpr auto kMaxAcceleration = 3_mps_sq;
|
||||
inline constexpr auto kMaxAngularSpeed = 3.142_rad_per_s;
|
||||
inline constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq;
|
||||
|
||||
inline constexpr double kPXController = 0.5;
|
||||
inline constexpr double kPYController = 0.5;
|
||||
inline constexpr double kPThetaController = 0.5;
|
||||
|
||||
//
|
||||
|
||||
extern const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints;
|
||||
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,32 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,43 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/ParallelRaceGroup.h>
|
||||
#include <frc2/command/RunCommand.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
class RobotContainer {
|
||||
public:
|
||||
RobotContainer();
|
||||
|
||||
frc2::CommandPtr GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,117 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/AnalogGyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/MecanumDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/ChassisSpeeds.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveDriveOdometry.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "SwerveModule.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1]
|
||||
* and the linear speeds have no effect on the angular speed.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction
|
||||
* (forward/backwards).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to
|
||||
* the field.
|
||||
*/
|
||||
void Drive(units::meters_per_second_t xSpeed,
|
||||
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
|
||||
bool fieldRelative,
|
||||
units::second_t period = DriveConstants::kDrivePeriod);
|
||||
|
||||
/**
|
||||
* Resets the drive encoders to currently read a position of 0.
|
||||
*/
|
||||
void ResetEncoders();
|
||||
|
||||
/**
|
||||
* Sets the drive MotorControllers to a power from -1 to 1.
|
||||
*/
|
||||
void SetModuleStates(wpi::array<frc::SwerveModuleState, 4> desiredStates);
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from 180 to 180
|
||||
*/
|
||||
units::degree_t GetHeading() const;
|
||||
|
||||
/**
|
||||
* Zeroes the heading of the robot.
|
||||
*/
|
||||
void ZeroHeading();
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
double GetTurnRate();
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
frc::Pose2d GetPose();
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
void ResetOdometry(frc::Pose2d pose);
|
||||
|
||||
units::meter_t kTrackwidth =
|
||||
0.5_m; // Distance between centers of right and left wheels on robot
|
||||
units::meter_t kWheelBase =
|
||||
0.7_m; // Distance between centers of front and back wheels on robot
|
||||
|
||||
frc::SwerveDriveKinematics<4> kDriveKinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
SwerveModule m_frontLeft;
|
||||
SwerveModule m_rearLeft;
|
||||
SwerveModule m_frontRight;
|
||||
SwerveModule m_rearRight;
|
||||
|
||||
// The gyro sensor
|
||||
frc::AnalogGyro m_gyro{0};
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
// 4 defines the number of modules
|
||||
frc::SwerveDriveOdometry<4> m_odometry;
|
||||
};
|
||||
@@ -1,57 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/Spark.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class SwerveModule {
|
||||
public:
|
||||
SwerveModule(int driveMotorChannel, int turningMotorChannel,
|
||||
const int driveEncoderPorts[2], const int turningEncoderPorts[2],
|
||||
bool driveEncoderReversed, bool turningEncoderReversed);
|
||||
|
||||
frc::SwerveModuleState GetState();
|
||||
|
||||
frc::SwerveModulePosition GetPosition();
|
||||
|
||||
void SetDesiredState(frc::SwerveModuleState& state);
|
||||
|
||||
void ResetEncoders();
|
||||
|
||||
private:
|
||||
// We have to use meters here instead of radians due to the fact that
|
||||
// ProfiledPIDController's constraints only take in meters per second and
|
||||
// meters per second squared.
|
||||
|
||||
static constexpr auto kModuleMaxAngularVelocity =
|
||||
units::radians_per_second_t{std::numbers::pi};
|
||||
static constexpr auto kModuleMaxAngularAcceleration =
|
||||
units::radians_per_second_squared_t{std::numbers::pi * 2.0};
|
||||
|
||||
frc::Spark m_driveMotor;
|
||||
frc::Spark m_turningMotor;
|
||||
|
||||
frc::Encoder m_driveEncoder;
|
||||
frc::Encoder m_turningEncoder;
|
||||
|
||||
frc::PIDController m_drivePIDController{
|
||||
ModuleConstants::kPModuleDriveController, 0, 0};
|
||||
frc::ProfiledPIDController<units::radians> m_turningPIDController{
|
||||
ModuleConstants::kPModuleTurningController,
|
||||
0.0,
|
||||
0.0,
|
||||
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
|
||||
};
|
||||
@@ -462,42 +462,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "MecanumControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Mecanum Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "MecanumControllerCommand",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2,
|
||||
"hasunittests": true
|
||||
},
|
||||
{
|
||||
"name": "SwerveControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a swerve drive using SwerveControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Swerve Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "SwerveControllerCommand",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2,
|
||||
"hasunittests": true
|
||||
},
|
||||
{
|
||||
"name": "DriveDistanceOffboard",
|
||||
"description": "Drive a differential drivetrain a set distance using TrapezoidProfile and smart motor controller PID.",
|
||||
|
||||
@@ -1,64 +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 <thread>
|
||||
|
||||
#include <frc/simulation/DriverStationSim.h>
|
||||
#include <frc/simulation/SimHooks.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <units/time.h>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
class MecanumControllerCommandTest : public testing::Test {
|
||||
Robot m_robot;
|
||||
std::optional<std::thread> m_thread;
|
||||
bool joystickWarning;
|
||||
|
||||
public:
|
||||
void SetUp() override {
|
||||
frc::sim::PauseTiming();
|
||||
joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
|
||||
frc::DriverStation::SilenceJoystickConnectionWarning(true);
|
||||
|
||||
m_thread = std::thread([&] { m_robot.StartCompetition(); });
|
||||
frc::sim::StepTiming(0.0_ms); // Wait for Notifiers
|
||||
}
|
||||
|
||||
void TearDown() override {
|
||||
m_robot.EndCompetition();
|
||||
m_thread->join();
|
||||
|
||||
frc::sim::DriverStationSim::ResetData();
|
||||
frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(MecanumControllerCommandTest, Match) {
|
||||
// auto
|
||||
frc::sim::DriverStationSim::SetAutonomous(true);
|
||||
frc::sim::DriverStationSim::SetEnabled(true);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
frc::sim::StepTiming(15_s);
|
||||
|
||||
// brief disabled period- exact duration shouldn't matter
|
||||
frc::sim::DriverStationSim::SetAutonomous(false);
|
||||
frc::sim::DriverStationSim::SetEnabled(false);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
frc::sim::StepTiming(3_s);
|
||||
|
||||
// teleop
|
||||
frc::sim::DriverStationSim::SetAutonomous(false);
|
||||
frc::sim::DriverStationSim::SetEnabled(true);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
frc::sim::StepTiming(135_s);
|
||||
|
||||
// end of match
|
||||
frc::sim::DriverStationSim::SetAutonomous(false);
|
||||
frc::sim::DriverStationSim::SetEnabled(false);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
}
|
||||
@@ -1,16 +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 <gtest/gtest.h>
|
||||
#include <hal/HALBase.h>
|
||||
|
||||
/**
|
||||
* Runs all unit tests.
|
||||
*/
|
||||
int main(int argc, char** argv) {
|
||||
HAL_Initialize(500, 0);
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
return ret;
|
||||
}
|
||||
@@ -1,69 +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 <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include <frc/simulation/DriverStationSim.h>
|
||||
#include <frc/simulation/SimHooks.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <units/time.h>
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
class SwerveControllerCommandTest : public testing::Test {
|
||||
Robot m_robot;
|
||||
std::optional<std::thread> m_thread;
|
||||
bool joystickWarning;
|
||||
|
||||
public:
|
||||
void SetUp() override {
|
||||
frc::sim::PauseTiming();
|
||||
joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
|
||||
frc::DriverStation::SilenceJoystickConnectionWarning(true);
|
||||
|
||||
m_thread = std::thread([&] { m_robot.StartCompetition(); });
|
||||
frc::sim::StepTiming(0.0_ms); // Wait for Notifiers
|
||||
}
|
||||
|
||||
void TearDown() override {
|
||||
m_robot.EndCompetition();
|
||||
m_thread->join();
|
||||
|
||||
frc::sim::DriverStationSim::ResetData();
|
||||
frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(SwerveControllerCommandTest, Match) {
|
||||
std::cerr << "autonomous" << std::endl;
|
||||
// auto
|
||||
frc::sim::DriverStationSim::SetAutonomous(true);
|
||||
frc::sim::DriverStationSim::SetEnabled(true);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
frc::sim::StepTiming(15_s);
|
||||
|
||||
// brief disabled period- exact duration shouldn't matter
|
||||
std::cerr << "mid disabled" << std::endl;
|
||||
frc::sim::DriverStationSim::SetAutonomous(false);
|
||||
frc::sim::DriverStationSim::SetEnabled(false);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
frc::sim::StepTiming(3_s);
|
||||
|
||||
// teleop
|
||||
std::cerr << "teleop" << std::endl;
|
||||
frc::sim::DriverStationSim::SetAutonomous(false);
|
||||
frc::sim::DriverStationSim::SetEnabled(true);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
frc::sim::StepTiming(135_s);
|
||||
|
||||
// end of match
|
||||
std::cerr << "end of match" << std::endl;
|
||||
frc::sim::DriverStationSim::SetAutonomous(false);
|
||||
frc::sim::DriverStationSim::SetEnabled(false);
|
||||
frc::sim::DriverStationSim::NotifyNewData();
|
||||
}
|
||||
@@ -1,16 +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 <gtest/gtest.h>
|
||||
#include <hal/HALBase.h>
|
||||
|
||||
/**
|
||||
* Runs all unit tests.
|
||||
*/
|
||||
int main(int argc, char** argv) {
|
||||
HAL_Initialize(500, 0);
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
return ret;
|
||||
}
|
||||
@@ -29,7 +29,6 @@ EXAMPLES_FOLDERS = [
|
||||
"i2ccommunication",
|
||||
"intermediatevision",
|
||||
"mecanumbot",
|
||||
"mecanumcontrollercommand",
|
||||
"mecanumdrive",
|
||||
"mecanumdriveposeestimator",
|
||||
"mechanism2d",
|
||||
@@ -46,7 +45,6 @@ EXAMPLES_FOLDERS = [
|
||||
"statespaceflywheel",
|
||||
"statespaceflywheelsysid",
|
||||
"swervebot",
|
||||
"swervecontrollercommand",
|
||||
"swervedriveposeestimator",
|
||||
"sysidroutine",
|
||||
"tankdrive",
|
||||
|
||||
@@ -475,42 +475,6 @@
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "MecanumControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Mecanum Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "mecanumcontrollercommand",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "SwerveControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a swerve drive using SwerveControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Swerve Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "swervecontrollercommand",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceFlywheel",
|
||||
"description": "Control a flywheel using a state-space model (based on values from CAD), with a Kalman Filter and LQR.",
|
||||
|
||||
@@ -1,87 +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.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be declared
|
||||
* globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class DriveConstants {
|
||||
public static final int kFrontLeftMotorPort = 0;
|
||||
public static final int kRearLeftMotorPort = 1;
|
||||
public static final int kFrontRightMotorPort = 2;
|
||||
public static final int kRearRightMotorPort = 3;
|
||||
|
||||
public static final int[] kFrontLeftEncoderPorts = new int[] {0, 1};
|
||||
public static final int[] kRearLeftEncoderPorts = new int[] {2, 3};
|
||||
public static final int[] kFrontRightEncoderPorts = new int[] {4, 5};
|
||||
public static final int[] kRearRightEncoderPorts = new int[] {6, 7};
|
||||
|
||||
public static final boolean kFrontLeftEncoderReversed = false;
|
||||
public static final boolean kRearLeftEncoderReversed = true;
|
||||
public static final boolean kFrontRightEncoderReversed = false;
|
||||
public static final boolean kRearRightEncoderReversed = true;
|
||||
|
||||
public static final double kTrackwidth = 0.5;
|
||||
// Distance between centers of right and left wheels on robot
|
||||
public static final double kWheelBase = 0.7;
|
||||
// Distance between centers of front and back wheels on robot
|
||||
|
||||
public static final MecanumDriveKinematics kDriveKinematics =
|
||||
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));
|
||||
|
||||
public static final int kEncoderCPR = 1024;
|
||||
public static final double kWheelDiameter = 0.15; // m
|
||||
public static final double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter * Math.PI) / kEncoderCPR;
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or theoretically
|
||||
// for *your* robot's drive.
|
||||
// The SysId tool provides a convenient method for obtaining these values for your robot.
|
||||
public static final SimpleMotorFeedforward kFeedforward =
|
||||
new SimpleMotorFeedforward(1, 0.8, 0.15);
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
public static final double kPFrontLeftVel = 0.5;
|
||||
public static final double kPRearLeftVel = 0.5;
|
||||
public static final double kPFrontRightVel = 0.5;
|
||||
public static final double kPRearRightVel = 0.5;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final double kMaxSpeed = 3; // m/s
|
||||
public static final double kMaxAcceleration = 3; // m/s²
|
||||
public static final double kMaxAngularSpeed = Math.PI; // rad/s
|
||||
public static final double kMaxAngularAcceleration = Math.PI; // rad/s²
|
||||
|
||||
public static final double kPXController = 0.5;
|
||||
public static final double kPYController = 0.5;
|
||||
public static final double kPThetaController = 0.5;
|
||||
|
||||
// Constraint for the motion profilied robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
|
||||
new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration);
|
||||
}
|
||||
}
|
||||
@@ -1,25 +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.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -1,100 +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.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
/**
|
||||
* The methods in this class are called automatically corresponding to each mode, as described in
|
||||
* the TimedRobot documentation. If you change the name of this class or the package after creating
|
||||
* this project, you must also update the Main.java file in the project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private final RobotContainer m_robotContainer;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be used for any
|
||||
* initialization code.
|
||||
*/
|
||||
public Robot() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/** This function is called once each time the robot enters Disabled mode. */
|
||||
@Override
|
||||
public void disabledInit() {}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
* = new MyAutoCommand(); break; case "Default Auto": default:
|
||||
* autonomousCommand = new ExampleCommand(); break; }
|
||||
*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().schedule(m_autonomousCommand);
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during autonomous. */
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during operator control. */
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
}
|
||||
@@ -1,130 +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.wpilibj.examples.mecanumcontrollercommand;
|
||||
|
||||
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.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.XboxController.Button;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
|
||||
* (including subsystems, commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
// The robot's subsystems
|
||||
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
|
||||
|
||||
// The driver's controller
|
||||
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
|
||||
|
||||
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||
public RobotContainer() {
|
||||
// Configure the button bindings
|
||||
configureButtonBindings();
|
||||
|
||||
// Configure default commands
|
||||
// Set the default drive command to split-stick arcade drive
|
||||
m_robotDrive.setDefaultCommand(
|
||||
// A split-stick arcade command, with forward/backward controlled by the left
|
||||
// hand, and turning controlled by the right.
|
||||
new RunCommand(
|
||||
() ->
|
||||
m_robotDrive.drive(
|
||||
-m_driverController.getLeftY(),
|
||||
-m_driverController.getRightX(),
|
||||
-m_driverController.getLeftX(),
|
||||
false),
|
||||
m_robotDrive));
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be created by
|
||||
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
|
||||
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
|
||||
* {@link JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
// Drive at half speed when the right bumper is held
|
||||
new JoystickButton(m_driverController, Button.kRightBumper.value)
|
||||
.onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5)))
|
||||
.onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// Create config for trajectory
|
||||
TrajectoryConfig config =
|
||||
new TrajectoryConfig(AutoConstants.kMaxSpeed, AutoConstants.kMaxAcceleration)
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
.setKinematics(DriveConstants.kDriveKinematics);
|
||||
|
||||
// An example trajectory to follow. All units in meters.
|
||||
Trajectory exampleTrajectory =
|
||||
TrajectoryGenerator.generateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
Pose2d.kZero,
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(3, 0, Rotation2d.kZero),
|
||||
config);
|
||||
|
||||
MecanumControllerCommand mecanumControllerCommand =
|
||||
new MecanumControllerCommand(
|
||||
exampleTrajectory,
|
||||
m_robotDrive::getPose,
|
||||
DriveConstants.kFeedforward,
|
||||
DriveConstants.kDriveKinematics,
|
||||
|
||||
// Position controllers
|
||||
new PIDController(AutoConstants.kPXController, 0, 0),
|
||||
new PIDController(AutoConstants.kPYController, 0, 0),
|
||||
new ProfiledPIDController(
|
||||
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints),
|
||||
|
||||
// Needed for normalizing wheel speeds
|
||||
AutoConstants.kMaxSpeed,
|
||||
|
||||
// Velocity PID's
|
||||
new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
|
||||
new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
|
||||
new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
|
||||
new PIDController(DriveConstants.kPRearRightVel, 0, 0),
|
||||
m_robotDrive::getCurrentWheelSpeeds,
|
||||
m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages
|
||||
m_robotDrive);
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return Commands.sequence(
|
||||
new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
|
||||
mecanumControllerCommand,
|
||||
new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
|
||||
}
|
||||
}
|
||||
@@ -1,239 +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.wpilibj.examples.mecanumcontrollercommand.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
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.util.sendable.SendableRegistry;
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.drive.MecanumDrive;
|
||||
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class DriveSubsystem extends SubsystemBase {
|
||||
private final PWMSparkMax m_frontLeft = new PWMSparkMax(DriveConstants.kFrontLeftMotorPort);
|
||||
private final PWMSparkMax m_rearLeft = new PWMSparkMax(DriveConstants.kRearLeftMotorPort);
|
||||
private final PWMSparkMax m_frontRight = new PWMSparkMax(DriveConstants.kFrontRightMotorPort);
|
||||
private final PWMSparkMax m_rearRight = new PWMSparkMax(DriveConstants.kRearRightMotorPort);
|
||||
|
||||
private final MecanumDrive m_drive =
|
||||
new MecanumDrive(m_frontLeft::set, m_rearLeft::set, m_frontRight::set, m_rearRight::set);
|
||||
|
||||
// The front-left-side drive encoder
|
||||
private final Encoder m_frontLeftEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kFrontLeftEncoderPorts[0],
|
||||
DriveConstants.kFrontLeftEncoderPorts[1],
|
||||
DriveConstants.kFrontLeftEncoderReversed);
|
||||
|
||||
// The rear-left-side drive encoder
|
||||
private final Encoder m_rearLeftEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kRearLeftEncoderPorts[0],
|
||||
DriveConstants.kRearLeftEncoderPorts[1],
|
||||
DriveConstants.kRearLeftEncoderReversed);
|
||||
|
||||
// The front-right--side drive encoder
|
||||
private final Encoder m_frontRightEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kFrontRightEncoderPorts[0],
|
||||
DriveConstants.kFrontRightEncoderPorts[1],
|
||||
DriveConstants.kFrontRightEncoderReversed);
|
||||
|
||||
// The rear-right-side drive encoder
|
||||
private final Encoder m_rearRightEncoder =
|
||||
new Encoder(
|
||||
DriveConstants.kRearRightEncoderPorts[0],
|
||||
DriveConstants.kRearRightEncoderPorts[1],
|
||||
DriveConstants.kRearRightEncoderReversed);
|
||||
|
||||
// The gyro sensor
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
MecanumDriveOdometry m_odometry =
|
||||
new MecanumDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_gyro.getRotation2d(),
|
||||
new MecanumDriveWheelPositions());
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
SendableRegistry.addChild(m_drive, m_frontLeft);
|
||||
SendableRegistry.addChild(m_drive, m_rearLeft);
|
||||
SendableRegistry.addChild(m_drive, m_frontRight);
|
||||
SendableRegistry.addChild(m_drive, m_rearRight);
|
||||
|
||||
// Sets the distance per pulse for the encoders
|
||||
m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_frontRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
// We need to invert one side of the drivetrain so that positive voltages
|
||||
// result in both sides moving forward. Depending on how your robot's
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_frontRight.setInverted(true);
|
||||
m_rearRight.setInverted(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(m_gyro.getRotation2d(), getCurrentWheelDistances());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_odometry.resetPosition(m_gyro.getRotation2d(), getCurrentWheelDistances(), pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear
|
||||
* speeds have no effect on the angular speed.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward/backwards).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
m_drive.driveCartesian(xSpeed, ySpeed, rot, m_gyro.getRotation2d());
|
||||
} else {
|
||||
m_drive.driveCartesian(xSpeed, ySpeed, rot);
|
||||
}
|
||||
}
|
||||
|
||||
/** Sets the front left drive MotorController to a voltage. */
|
||||
public void setDriveMotorControllersVolts(
|
||||
double frontLeftVoltage,
|
||||
double frontRightVoltage,
|
||||
double rearLeftVoltage,
|
||||
double rearRightVoltage) {
|
||||
m_frontLeft.setVoltage(frontLeftVoltage);
|
||||
m_rearLeft.setVoltage(rearLeftVoltage);
|
||||
m_frontRight.setVoltage(frontRightVoltage);
|
||||
m_rearRight.setVoltage(rearRightVoltage);
|
||||
}
|
||||
|
||||
/** Resets the drive encoders to currently read a position of 0. */
|
||||
public void resetEncoders() {
|
||||
m_frontLeftEncoder.reset();
|
||||
m_rearLeftEncoder.reset();
|
||||
m_frontRightEncoder.reset();
|
||||
m_rearRightEncoder.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the front left drive encoder.
|
||||
*
|
||||
* @return the front left drive encoder
|
||||
*/
|
||||
public Encoder getFrontLeftEncoder() {
|
||||
return m_frontLeftEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the rear left drive encoder.
|
||||
*
|
||||
* @return the rear left drive encoder
|
||||
*/
|
||||
public Encoder getRearLeftEncoder() {
|
||||
return m_rearLeftEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the front right drive encoder.
|
||||
*
|
||||
* @return the front right drive encoder
|
||||
*/
|
||||
public Encoder getFrontRightEncoder() {
|
||||
return m_frontRightEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the rear right drive encoder.
|
||||
*
|
||||
* @return the rear right encoder
|
||||
*/
|
||||
public Encoder getRearRightEncoder() {
|
||||
return m_rearRightEncoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current wheel speeds.
|
||||
*
|
||||
* @return the current wheel speeds in a MecanumDriveWheelSpeeds object.
|
||||
*/
|
||||
public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
|
||||
return new MecanumDriveWheelSpeeds(
|
||||
m_frontLeftEncoder.getRate(),
|
||||
m_rearLeftEncoder.getRate(),
|
||||
m_frontRightEncoder.getRate(),
|
||||
m_rearRightEncoder.getRate());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current wheel distance measurements.
|
||||
*
|
||||
* @return the current wheel distance measurements in a MecanumDriveWheelPositions object.
|
||||
*/
|
||||
public MecanumDriveWheelPositions getCurrentWheelDistances() {
|
||||
return new MecanumDriveWheelPositions(
|
||||
m_frontLeftEncoder.getDistance(),
|
||||
m_rearLeftEncoder.getDistance(),
|
||||
m_frontRightEncoder.getDistance(),
|
||||
m_rearRightEncoder.getDistance());
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
|
||||
*
|
||||
* @param maxOutput the maximum output to which the drive will be constrained
|
||||
*/
|
||||
public void setMaxOutput(double maxOutput) {
|
||||
m_drive.setMaxOutput(maxOutput);
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
m_gyro.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return m_gyro.getRotation2d().getDegrees();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
return -m_gyro.getRate();
|
||||
}
|
||||
}
|
||||
@@ -1,116 +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.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be declared
|
||||
* globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class DriveConstants {
|
||||
public static final int kFrontLeftDriveMotorPort = 0;
|
||||
public static final int kRearLeftDriveMotorPort = 2;
|
||||
public static final int kFrontRightDriveMotorPort = 4;
|
||||
public static final int kRearRightDriveMotorPort = 6;
|
||||
|
||||
public static final int kFrontLeftTurningMotorPort = 1;
|
||||
public static final int kRearLeftTurningMotorPort = 3;
|
||||
public static final int kFrontRightTurningMotorPort = 5;
|
||||
public static final int kRearRightTurningMotorPort = 7;
|
||||
|
||||
public static final int[] kFrontLeftTurningEncoderPorts = new int[] {0, 1};
|
||||
public static final int[] kRearLeftTurningEncoderPorts = new int[] {2, 3};
|
||||
public static final int[] kFrontRightTurningEncoderPorts = new int[] {4, 5};
|
||||
public static final int[] kRearRightTurningEncoderPorts = new int[] {6, 7};
|
||||
|
||||
public static final boolean kFrontLeftTurningEncoderReversed = false;
|
||||
public static final boolean kRearLeftTurningEncoderReversed = true;
|
||||
public static final boolean kFrontRightTurningEncoderReversed = false;
|
||||
public static final boolean kRearRightTurningEncoderReversed = true;
|
||||
|
||||
public static final int[] kFrontLeftDriveEncoderPorts = new int[] {8, 9};
|
||||
public static final int[] kRearLeftDriveEncoderPorts = new int[] {10, 11};
|
||||
public static final int[] kFrontRightDriveEncoderPorts = new int[] {12, 13};
|
||||
public static final int[] kRearRightDriveEncoderPorts = new int[] {14, 15};
|
||||
|
||||
public static final boolean kFrontLeftDriveEncoderReversed = false;
|
||||
public static final boolean kRearLeftDriveEncoderReversed = true;
|
||||
public static final boolean kFrontRightDriveEncoderReversed = false;
|
||||
public static final boolean kRearRightDriveEncoderReversed = true;
|
||||
|
||||
// If you call DriveSubsystem.drive() with a different period make sure to update this.
|
||||
public static final double kDrivePeriod = TimedRobot.kDefaultPeriod;
|
||||
|
||||
public static final double kTrackwidth = 0.5;
|
||||
// Distance between centers of right and left wheels on robot
|
||||
public static final double kWheelBase = 0.7;
|
||||
// Distance between front and back wheels on robot
|
||||
public static final SwerveDriveKinematics kDriveKinematics =
|
||||
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));
|
||||
|
||||
public static final boolean kGyroReversed = false;
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or theoretically
|
||||
// for *your* robot's drive.
|
||||
// The SysId tool provides a convenient method for obtaining these values for your robot.
|
||||
public static final double ks = 1; // V
|
||||
public static final double kv = 0.8; // V/(m/s)
|
||||
public static final double ka = 0.15; // V/(m/s²)
|
||||
|
||||
public static final double kMaxSpeed = 3; // m/s
|
||||
}
|
||||
|
||||
public static final class ModuleConstants {
|
||||
public static final double kMaxModuleAngularSpeed = 2 * Math.PI; // rad/s
|
||||
public static final double kMaxModuleAngularAcceleration = 2 * Math.PI; // rad/s²
|
||||
|
||||
public static final int kEncoderCPR = 1024;
|
||||
public static final double kWheelDiameter = 0.15; // m
|
||||
public static final double kDriveEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter * Math.PI) / kEncoderCPR;
|
||||
|
||||
public static final double kTurningEncoderDistancePerPulse =
|
||||
// Assumes the encoders are on a 1:1 reduction with the module shaft.
|
||||
(2 * Math.PI) / kEncoderCPR;
|
||||
|
||||
public static final double kPModuleTurningController = 1;
|
||||
|
||||
public static final double kPModuleDriveController = 1;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final double kMaxSpeed = 3; // m/s
|
||||
public static final double kMaxAcceleration = 3; // m/s²
|
||||
public static final double kMaxAngularSpeed = Math.PI; // rad/s
|
||||
public static final double kMaxAngularAcceleration = Math.PI; // rad/s²
|
||||
|
||||
public static final double kPXController = 1;
|
||||
public static final double kPYController = 1;
|
||||
public static final double kPThetaController = 1;
|
||||
|
||||
// Constraint for the motion profiled robot angle controller
|
||||
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
|
||||
new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration);
|
||||
}
|
||||
}
|
||||
@@ -1,25 +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.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -1,100 +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.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
/**
|
||||
* The methods in this class are called automatically corresponding to each mode, as described in
|
||||
* the TimedRobot documentation. If you change the name of this class or the package after creating
|
||||
* this project, you must also update the Main.java file in the project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private final RobotContainer m_robotContainer;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be used for any
|
||||
* initialization code.
|
||||
*/
|
||||
public Robot() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/** This function is called once each time the robot enters Disabled mode. */
|
||||
@Override
|
||||
public void disabledInit() {}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*
|
||||
* String autoSelected = SmartDashboard.getString("Auto Selector",
|
||||
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
|
||||
* = new MyAutoCommand(); break; case "Default Auto": default:
|
||||
* autonomousCommand = new ExampleCommand(); break; }
|
||||
*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
CommandScheduler.getInstance().schedule(m_autonomousCommand);
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during autonomous. */
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
}
|
||||
|
||||
/** This function is called periodically during operator control. */
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
}
|
||||
@@ -1,120 +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.wpilibj.examples.swervecontrollercommand;
|
||||
|
||||
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.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
|
||||
* (including subsystems, commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
// The robot's subsystems
|
||||
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
|
||||
|
||||
// The driver's controller
|
||||
XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
|
||||
|
||||
/** The container for the robot. Contains subsystems, OI devices, and commands. */
|
||||
public RobotContainer() {
|
||||
// Configure the button bindings
|
||||
configureButtonBindings();
|
||||
|
||||
// Configure default commands
|
||||
m_robotDrive.setDefaultCommand(
|
||||
// The left stick controls translation of the robot.
|
||||
// Turning is controlled by the X axis of the right stick.
|
||||
new RunCommand(
|
||||
() ->
|
||||
m_robotDrive.drive(
|
||||
// Multiply by max speed to map the joystick unitless inputs to actual units.
|
||||
// This will map the [-1, 1] to [max speed backwards, max speed forwards],
|
||||
// converting them to actual units.
|
||||
m_driverController.getLeftY() * DriveConstants.kMaxSpeed,
|
||||
m_driverController.getLeftX() * DriveConstants.kMaxSpeed,
|
||||
m_driverController.getRightX() * ModuleConstants.kMaxModuleAngularSpeed,
|
||||
false),
|
||||
m_robotDrive));
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be created by
|
||||
* instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
|
||||
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
|
||||
* {@link JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// Create config for trajectory
|
||||
TrajectoryConfig config =
|
||||
new TrajectoryConfig(AutoConstants.kMaxSpeed, AutoConstants.kMaxAcceleration)
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
.setKinematics(DriveConstants.kDriveKinematics);
|
||||
|
||||
// An example trajectory to follow. All units in meters.
|
||||
Trajectory exampleTrajectory =
|
||||
TrajectoryGenerator.generateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
Pose2d.kZero,
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(3, 0, Rotation2d.kZero),
|
||||
config);
|
||||
|
||||
var thetaController =
|
||||
new ProfiledPIDController(
|
||||
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
SwerveControllerCommand swerveControllerCommand =
|
||||
new SwerveControllerCommand(
|
||||
exampleTrajectory,
|
||||
m_robotDrive::getPose, // Functional interface to feed supplier
|
||||
DriveConstants.kDriveKinematics,
|
||||
|
||||
// Position controllers
|
||||
new PIDController(AutoConstants.kPXController, 0, 0),
|
||||
new PIDController(AutoConstants.kPYController, 0, 0),
|
||||
thetaController,
|
||||
m_robotDrive::setModuleStates,
|
||||
m_robotDrive);
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return Commands.sequence(
|
||||
new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
|
||||
swerveControllerCommand,
|
||||
new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
|
||||
}
|
||||
}
|
||||
@@ -1,179 +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.wpilibj.examples.swervecontrollercommand.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
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.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class DriveSubsystem extends SubsystemBase {
|
||||
// Robot swerve modules
|
||||
private final SwerveModule m_frontLeft =
|
||||
new SwerveModule(
|
||||
DriveConstants.kFrontLeftDriveMotorPort,
|
||||
DriveConstants.kFrontLeftTurningMotorPort,
|
||||
DriveConstants.kFrontLeftDriveEncoderPorts,
|
||||
DriveConstants.kFrontLeftTurningEncoderPorts,
|
||||
DriveConstants.kFrontLeftDriveEncoderReversed,
|
||||
DriveConstants.kFrontLeftTurningEncoderReversed);
|
||||
|
||||
private final SwerveModule m_rearLeft =
|
||||
new SwerveModule(
|
||||
DriveConstants.kRearLeftDriveMotorPort,
|
||||
DriveConstants.kRearLeftTurningMotorPort,
|
||||
DriveConstants.kRearLeftDriveEncoderPorts,
|
||||
DriveConstants.kRearLeftTurningEncoderPorts,
|
||||
DriveConstants.kRearLeftDriveEncoderReversed,
|
||||
DriveConstants.kRearLeftTurningEncoderReversed);
|
||||
|
||||
private final SwerveModule m_frontRight =
|
||||
new SwerveModule(
|
||||
DriveConstants.kFrontRightDriveMotorPort,
|
||||
DriveConstants.kFrontRightTurningMotorPort,
|
||||
DriveConstants.kFrontRightDriveEncoderPorts,
|
||||
DriveConstants.kFrontRightTurningEncoderPorts,
|
||||
DriveConstants.kFrontRightDriveEncoderReversed,
|
||||
DriveConstants.kFrontRightTurningEncoderReversed);
|
||||
|
||||
private final SwerveModule m_rearRight =
|
||||
new SwerveModule(
|
||||
DriveConstants.kRearRightDriveMotorPort,
|
||||
DriveConstants.kRearRightTurningMotorPort,
|
||||
DriveConstants.kRearRightDriveEncoderPorts,
|
||||
DriveConstants.kRearRightTurningEncoderPorts,
|
||||
DriveConstants.kRearRightDriveEncoderReversed,
|
||||
DriveConstants.kRearRightTurningEncoderReversed);
|
||||
|
||||
// The gyro sensor
|
||||
private final AnalogGyro m_gyro = new AnalogGyro(0);
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
SwerveDriveOdometry m_odometry =
|
||||
new SwerveDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// Update the odometry in the periodic block
|
||||
m_odometry.update(
|
||||
m_gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_odometry.resetPosition(
|
||||
m_gyro.getRotation2d(),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
m_frontRight.getPosition(),
|
||||
m_rearLeft.getPosition(),
|
||||
m_rearRight.getPosition()
|
||||
},
|
||||
pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
|
||||
if (fieldRelative) {
|
||||
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
|
||||
}
|
||||
chassisSpeeds = chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
|
||||
|
||||
var states = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeed);
|
||||
|
||||
m_frontLeft.setDesiredState(states[0]);
|
||||
m_frontRight.setDesiredState(states[1]);
|
||||
m_rearLeft.setDesiredState(states[2]);
|
||||
m_rearRight.setDesiredState(states[3]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the swerve ModuleStates.
|
||||
*
|
||||
* @param desiredStates The desired SwerveModule states.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeed);
|
||||
m_frontLeft.setDesiredState(desiredStates[0]);
|
||||
m_frontRight.setDesiredState(desiredStates[1]);
|
||||
m_rearLeft.setDesiredState(desiredStates[2]);
|
||||
m_rearRight.setDesiredState(desiredStates[3]);
|
||||
}
|
||||
|
||||
/** Resets the drive encoders to currently read a position of 0. */
|
||||
public void resetEncoders() {
|
||||
m_frontLeft.resetEncoders();
|
||||
m_rearLeft.resetEncoders();
|
||||
m_frontRight.resetEncoders();
|
||||
m_rearRight.resetEncoders();
|
||||
}
|
||||
|
||||
/** Zeroes the heading of the robot. */
|
||||
public void zeroHeading() {
|
||||
m_gyro.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return m_gyro.getRotation2d().getDegrees();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
@@ -1,137 +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.wpilibj.examples.swervecontrollercommand.subsystems;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
|
||||
public class SwerveModule {
|
||||
private final Spark m_driveMotor;
|
||||
private final Spark m_turningMotor;
|
||||
|
||||
private final Encoder m_driveEncoder;
|
||||
private final Encoder m_turningEncoder;
|
||||
|
||||
private final PIDController m_drivePIDController =
|
||||
new PIDController(ModuleConstants.kPModuleDriveController, 0, 0);
|
||||
|
||||
// Using a TrapezoidProfile PIDController to allow for smooth turning
|
||||
private final ProfiledPIDController m_turningPIDController =
|
||||
new ProfiledPIDController(
|
||||
ModuleConstants.kPModuleTurningController,
|
||||
0,
|
||||
0,
|
||||
new TrapezoidProfile.Constraints(
|
||||
ModuleConstants.kMaxModuleAngularSpeed,
|
||||
ModuleConstants.kMaxModuleAngularAcceleration));
|
||||
|
||||
/**
|
||||
* Constructs a SwerveModule.
|
||||
*
|
||||
* @param driveMotorChannel The channel of the drive motor.
|
||||
* @param turningMotorChannel The channel of the turning motor.
|
||||
* @param driveEncoderChannels The channels of the drive encoder.
|
||||
* @param turningEncoderChannels The channels of the turning encoder.
|
||||
* @param driveEncoderReversed Whether the drive encoder is reversed.
|
||||
* @param turningEncoderReversed Whether the turning encoder is reversed.
|
||||
*/
|
||||
public SwerveModule(
|
||||
int driveMotorChannel,
|
||||
int turningMotorChannel,
|
||||
int[] driveEncoderChannels,
|
||||
int[] turningEncoderChannels,
|
||||
boolean driveEncoderReversed,
|
||||
boolean turningEncoderReversed) {
|
||||
m_driveMotor = new Spark(driveMotorChannel);
|
||||
m_turningMotor = new Spark(turningMotorChannel);
|
||||
|
||||
m_driveEncoder = new Encoder(driveEncoderChannels[0], driveEncoderChannels[1]);
|
||||
|
||||
m_turningEncoder = new Encoder(turningEncoderChannels[0], turningEncoderChannels[1]);
|
||||
|
||||
// Set the distance per pulse for the drive encoder. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_driveEncoder.setDistancePerPulse(ModuleConstants.kDriveEncoderDistancePerPulse);
|
||||
|
||||
// Set whether drive encoder should be reversed or not
|
||||
m_driveEncoder.setReverseDirection(driveEncoderReversed);
|
||||
|
||||
// Set the distance (in this case, angle) in radians per pulse for the turning encoder.
|
||||
// This is the the angle through an entire rotation (2 * pi) divided by the
|
||||
// encoder resolution.
|
||||
m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse);
|
||||
|
||||
// Set whether turning encoder should be reversed or not
|
||||
m_turningEncoder.setReverseDirection(turningEncoderReversed);
|
||||
|
||||
// Limit the PID Controller's input range between -pi and pi and set the input
|
||||
// to be continuous.
|
||||
m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current position of the module.
|
||||
*
|
||||
* @return The current position of the module.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(
|
||||
m_driveEncoder.getDistance(), new Rotation2d(m_turningEncoder.getDistance()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the desired state for the module.
|
||||
*
|
||||
* @param desiredState Desired state with speed and angle.
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees
|
||||
desiredState.optimize(encoderRotation);
|
||||
|
||||
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
|
||||
// direction of travel that can occur when modules change directions. This results in smoother
|
||||
// driving.
|
||||
desiredState.cosineScale(encoderRotation);
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
final double driveOutput =
|
||||
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
final double turnOutput =
|
||||
m_turningPIDController.calculate(
|
||||
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
m_driveMotor.set(driveOutput);
|
||||
m_turningMotor.set(turnOutput);
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_driveEncoder.reset();
|
||||
m_turningEncoder.reset();
|
||||
}
|
||||
}
|
||||
@@ -1,168 +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.math.controller;
|
||||
|
||||
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.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
|
||||
/**
|
||||
* This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
|
||||
* (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve
|
||||
* compared to skid-steer style drivetrains because it is possible to individually control
|
||||
* field-relative x, y, and angular velocity.
|
||||
*
|
||||
* <p>The holonomic drive controller takes in one PID controller for each direction, field-relative
|
||||
* x and y, and one profiled PID controller for the angular direction. Because the heading dynamics
|
||||
* are decoupled from translations, users can specify a custom heading that the drivetrain should
|
||||
* point toward. This heading reference is profiled for smoothness.
|
||||
*/
|
||||
public class HolonomicDriveController {
|
||||
private Pose2d m_poseError = Pose2d.kZero;
|
||||
private Rotation2d m_rotationError = Rotation2d.kZero;
|
||||
private Pose2d m_poseTolerance = Pose2d.kZero;
|
||||
private boolean m_enabled = true;
|
||||
|
||||
private final PIDController m_xController;
|
||||
private final PIDController m_yController;
|
||||
private final ProfiledPIDController m_thetaController;
|
||||
|
||||
private boolean m_firstRun = true;
|
||||
|
||||
/**
|
||||
* Constructs a holonomic drive controller.
|
||||
*
|
||||
* @param xController A PID Controller to respond to error in the field-relative x direction.
|
||||
* @param yController A PID Controller to respond to error in the field-relative y direction.
|
||||
* @param thetaController A profiled PID controller to respond to error in angle.
|
||||
*/
|
||||
public HolonomicDriveController(
|
||||
PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
|
||||
m_xController = xController;
|
||||
m_yController = yController;
|
||||
m_thetaController = thetaController;
|
||||
m_thetaController.enableContinuousInput(0, Units.degreesToRadians(360.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*
|
||||
* @return True if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
public boolean atReference() {
|
||||
final var eTranslate = m_poseError.getTranslation();
|
||||
final var eRotate = m_rotationError;
|
||||
final var tolTranslate = m_poseTolerance.getTranslation();
|
||||
final var tolRotate = m_poseTolerance.getRotation();
|
||||
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
|
||||
&& Math.abs(eTranslate.getY()) < tolTranslate.getY()
|
||||
&& Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerance for use with atReference().
|
||||
*
|
||||
* @param tolerance The pose error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(Pose2d tolerance) {
|
||||
m_poseTolerance = tolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose, as measured by odometry or pose estimator.
|
||||
* @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
|
||||
* @param desiredLinearVelocity The desired linear velocity in m/s.
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose,
|
||||
Pose2d trajectoryPose,
|
||||
double desiredLinearVelocity,
|
||||
Rotation2d desiredHeading) {
|
||||
// If this is the first run, then we need to reset the theta controller to the current pose's
|
||||
// heading.
|
||||
if (m_firstRun) {
|
||||
m_thetaController.reset(currentPose.getRotation().getRadians());
|
||||
m_firstRun = false;
|
||||
}
|
||||
|
||||
// Calculate feedforward velocities (field-relative).
|
||||
double xFF = desiredLinearVelocity * trajectoryPose.getRotation().getCos();
|
||||
double yFF = desiredLinearVelocity * trajectoryPose.getRotation().getSin();
|
||||
double thetaFF =
|
||||
m_thetaController.calculate(
|
||||
currentPose.getRotation().getRadians(), desiredHeading.getRadians());
|
||||
|
||||
m_poseError = trajectoryPose.relativeTo(currentPose);
|
||||
m_rotationError = desiredHeading.minus(currentPose.getRotation());
|
||||
|
||||
if (!m_enabled) {
|
||||
return new ChassisSpeeds(xFF, yFF, thetaFF).toRobotRelative(currentPose.getRotation());
|
||||
}
|
||||
|
||||
// Calculate feedback velocities (based on position error).
|
||||
double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
|
||||
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
|
||||
|
||||
// Return next output.
|
||||
return new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF)
|
||||
.toRobotRelative(currentPose.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose, as measured by odometry or pose estimator.
|
||||
* @param desiredState The desired trajectory pose, as sampled for the current timestep.
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
|
||||
return calculate(currentPose, desiredState.pose, desiredState.velocity, desiredHeading);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting problems. When calculate() is called on
|
||||
* a disabled controller, only feedforward values are returned.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
public void setEnabled(boolean enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the x controller.
|
||||
*
|
||||
* @return X PIDController
|
||||
*/
|
||||
public PIDController getXController() {
|
||||
return m_xController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the y controller.
|
||||
*
|
||||
* @return Y PIDController
|
||||
*/
|
||||
public PIDController getYController() {
|
||||
return m_yController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the heading controller.
|
||||
*
|
||||
* @return heading ProfiledPIDController
|
||||
*/
|
||||
public ProfiledPIDController getThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
}
|
||||
@@ -1,53 +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.math.kinematics;
|
||||
|
||||
/**
|
||||
* Represents the motor voltages for a mecanum drive drivetrain.
|
||||
*
|
||||
* @deprecated Use {@link
|
||||
* edu.wpi.first.wpilibj2.command.MecanumControllerCommand.MecanumVoltagesConsumer}
|
||||
*/
|
||||
@Deprecated(since = "2025", forRemoval = true)
|
||||
public class MecanumDriveMotorVoltages {
|
||||
/** Voltage of the front left motor. */
|
||||
public double frontLeft;
|
||||
|
||||
/** Voltage of the front right motor. */
|
||||
public double frontRight;
|
||||
|
||||
/** Voltage of the rear left motor. */
|
||||
public double rearLeft;
|
||||
|
||||
/** Voltage of the rear right motor. */
|
||||
public double rearRight;
|
||||
|
||||
/** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
|
||||
public MecanumDriveMotorVoltages() {}
|
||||
|
||||
/**
|
||||
* Constructs a MecanumDriveMotorVoltages.
|
||||
*
|
||||
* @param frontLeft Voltage of the front left motor.
|
||||
* @param frontRight Voltage of the front right motor.
|
||||
* @param rearLeft Voltage of the rear left motor.
|
||||
* @param rearRight Voltage of the rear right motor.
|
||||
*/
|
||||
public MecanumDriveMotorVoltages(
|
||||
double frontLeft, double frontRight, double rearLeft, double rearRight) {
|
||||
this.frontLeft = frontLeft;
|
||||
this.frontRight = frontRight;
|
||||
this.rearLeft = rearLeft;
|
||||
this.rearRight = rearRight;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format(
|
||||
"MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
|
||||
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
|
||||
frontLeft, frontRight, rearLeft, rearRight);
|
||||
}
|
||||
}
|
||||
@@ -1,218 +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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc/controller/ProfiledPIDController.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/geometry/Rotation2d.h"
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/angle.h"
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* This holonomic drive controller can be used to follow trajectories using a
|
||||
* holonomic drivetrain (i.e. swerve or mecanum). Holonomic trajectory following
|
||||
* is a much simpler problem to solve compared to skid-steer style drivetrains
|
||||
* because it is possible to individually control field-relative x, y, and
|
||||
* angular velocity.
|
||||
*
|
||||
* The holonomic drive controller takes in one PID controller for each
|
||||
* direction, field-relative x and y, and one profiled PID controller for the
|
||||
* angular direction. Because the heading dynamics are decoupled from
|
||||
* translations, users can specify a custom heading that the drivetrain should
|
||||
* point toward. This heading reference is profiled for smoothness.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT HolonomicDriveController {
|
||||
public:
|
||||
/**
|
||||
* Constructs a holonomic drive controller.
|
||||
*
|
||||
* @param xController A PID Controller to respond to error in the
|
||||
* field-relative x direction.
|
||||
* @param yController A PID Controller to respond to error in the
|
||||
* field-relative y direction.
|
||||
* @param thetaController A profiled PID controller to respond to error in
|
||||
* angle.
|
||||
*/
|
||||
constexpr HolonomicDriveController(
|
||||
PIDController xController, PIDController yController,
|
||||
ProfiledPIDController<units::radian> thetaController)
|
||||
: m_xController(std::move(xController)),
|
||||
m_yController(std::move(yController)),
|
||||
m_thetaController(std::move(thetaController)) {
|
||||
m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
|
||||
}
|
||||
|
||||
constexpr HolonomicDriveController(const HolonomicDriveController&) = default;
|
||||
constexpr HolonomicDriveController& operator=(
|
||||
const HolonomicDriveController&) = default;
|
||||
constexpr HolonomicDriveController(HolonomicDriveController&&) = default;
|
||||
constexpr HolonomicDriveController& operator=(HolonomicDriveController&&) =
|
||||
default;
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
constexpr bool AtReference() const {
|
||||
const auto& eTranslate = m_poseError.Translation();
|
||||
const auto& eRotate = m_rotationError;
|
||||
const auto& tolTranslate = m_poseTolerance.Translation();
|
||||
const auto& tolRotate = m_poseTolerance.Rotation();
|
||||
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
|
||||
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
|
||||
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with
|
||||
* AtReference().
|
||||
*
|
||||
* @param tolerance Pose error which is tolerable.
|
||||
*/
|
||||
constexpr void SetTolerance(const Pose2d& tolerance) {
|
||||
m_poseTolerance = tolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose, as measured by odometry or pose
|
||||
* estimator.
|
||||
* @param trajectoryPose The desired trajectory pose, as sampled for the
|
||||
* current timestep.
|
||||
* @param desiredLinearVelocity The desired linear velocity.
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
constexpr ChassisSpeeds Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& trajectoryPose,
|
||||
units::meters_per_second_t desiredLinearVelocity,
|
||||
const Rotation2d& desiredHeading) {
|
||||
// If this is the first run, then we need to reset the theta controller to
|
||||
// the current pose's heading.
|
||||
if (m_firstRun) {
|
||||
m_thetaController.Reset(currentPose.Rotation().Radians());
|
||||
m_firstRun = false;
|
||||
}
|
||||
|
||||
// Calculate feedforward velocities (field-relative)
|
||||
auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
|
||||
auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
|
||||
auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
|
||||
currentPose.Rotation().Radians(), desiredHeading.Radians())};
|
||||
|
||||
m_poseError = trajectoryPose.RelativeTo(currentPose);
|
||||
m_rotationError = desiredHeading - currentPose.Rotation();
|
||||
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds{xFF, yFF, thetaFF}.ToRobotRelative(
|
||||
currentPose.Rotation());
|
||||
}
|
||||
|
||||
// Calculate feedback velocities (based on position error).
|
||||
auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
|
||||
currentPose.X().value(), trajectoryPose.X().value())};
|
||||
auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
|
||||
currentPose.Y().value(), trajectoryPose.Y().value())};
|
||||
|
||||
// Return next output.
|
||||
return ChassisSpeeds{xFF + xFeedback, yFF + yFeedback, thetaFF}
|
||||
.ToRobotRelative(currentPose.Rotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose, as measured by odometry or pose
|
||||
* estimator.
|
||||
* @param desiredState The desired trajectory pose, as sampled for the current
|
||||
* timestep.
|
||||
* @param desiredHeading The desired heading.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
constexpr ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState,
|
||||
const Rotation2d& desiredHeading) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredHeading);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes. When
|
||||
* Calculate() is called on a disabled controller, only feedforward values
|
||||
* are returned.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
constexpr void SetEnabled(bool enabled) { m_enabled = enabled; }
|
||||
|
||||
/**
|
||||
* Returns the X PIDController
|
||||
*
|
||||
* @deprecated Use GetXController() instead.
|
||||
*/
|
||||
[[deprecated("Use GetXController() instead")]]
|
||||
constexpr PIDController& getXController() {
|
||||
return m_xController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y PIDController
|
||||
*
|
||||
* @deprecated Use GetYController() instead.
|
||||
*/
|
||||
[[deprecated("Use GetYController() instead")]]
|
||||
constexpr PIDController& getYController() {
|
||||
return m_yController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotation ProfiledPIDController
|
||||
*
|
||||
* @deprecated Use GetThetaController() instead.
|
||||
*/
|
||||
[[deprecated("Use GetThetaController() instead")]]
|
||||
constexpr ProfiledPIDController<units::radian>& getThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the X PIDController
|
||||
*/
|
||||
constexpr PIDController& GetXController() { return m_xController; }
|
||||
|
||||
/**
|
||||
* Returns the Y PIDController
|
||||
*/
|
||||
constexpr PIDController& GetYController() { return m_yController; }
|
||||
|
||||
/**
|
||||
* Returns the rotation ProfiledPIDController
|
||||
*/
|
||||
constexpr ProfiledPIDController<units::radian>& GetThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
|
||||
private:
|
||||
Pose2d m_poseError;
|
||||
Rotation2d m_rotationError;
|
||||
Pose2d m_poseTolerance;
|
||||
bool m_enabled = true;
|
||||
|
||||
PIDController m_xController;
|
||||
PIDController m_yController;
|
||||
ProfiledPIDController<units::radian> m_thetaController;
|
||||
|
||||
bool m_firstRun = true;
|
||||
};
|
||||
} // namespace frc
|
||||
@@ -1,85 +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.math.controller;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Twist2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class HolonomicDriveControllerTest {
|
||||
private static final double kTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = Math.toRadians(2);
|
||||
|
||||
@Test
|
||||
void testReachesReference() {
|
||||
HolonomicDriveController controller =
|
||||
new HolonomicDriveController(
|
||||
new PIDController(1.0, 0.0, 0.0),
|
||||
new PIDController(1.0, 0.0, 0.0),
|
||||
new ProfiledPIDController(
|
||||
1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.0 * Math.PI, Math.PI)));
|
||||
Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
|
||||
|
||||
List<Pose2d> waypoints = new ArrayList<>();
|
||||
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
|
||||
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
|
||||
|
||||
TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
|
||||
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
final double kDt = 0.02;
|
||||
final double kTotalTime = trajectory.getTotalTime();
|
||||
|
||||
for (int i = 0; i < (kTotalTime / kDt); i++) {
|
||||
Trajectory.State state = trajectory.sample(kDt * i);
|
||||
ChassisSpeeds output = controller.calculate(robotPose, state, Rotation2d.kZero);
|
||||
|
||||
robotPose = robotPose.exp(new Twist2d(output.vx * kDt, output.vy * kDt, output.omega * kDt));
|
||||
}
|
||||
|
||||
final List<Trajectory.State> states = trajectory.getStates();
|
||||
final Pose2d endPose = states.get(states.size() - 1).pose;
|
||||
|
||||
// Java lambdas require local variables referenced from a lambda expression
|
||||
// must be final or effectively final.
|
||||
final Pose2d finalRobotPose = robotPose;
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
|
||||
() -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
0.0,
|
||||
MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
|
||||
kAngularTolerance));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDoesNotRotateUnnecessarily() {
|
||||
var controller =
|
||||
new HolonomicDriveController(
|
||||
new PIDController(1, 0, 0),
|
||||
new PIDController(1, 0, 0),
|
||||
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
|
||||
|
||||
ChassisSpeeds speeds =
|
||||
controller.calculate(
|
||||
new Pose2d(0, 0, new Rotation2d(1.57)), Pose2d.kZero, 0, new Rotation2d(1.57));
|
||||
|
||||
assertEquals(0.0, speeds.omega);
|
||||
}
|
||||
}
|
||||
@@ -1,67 +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 <numbers>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "frc/controller/HolonomicDriveController.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "units/angular_acceleration.h"
|
||||
#include "units/math.h"
|
||||
#include "units/time.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
static constexpr units::meter_t kTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
|
||||
180.0};
|
||||
|
||||
TEST(HolonomicDriveControllerTest, ReachesReference) {
|
||||
frc::HolonomicDriveController controller{
|
||||
frc::PIDController{1.0, 0.0, 0.0}, frc::PIDController{1.0, 0.0, 0.0},
|
||||
frc::ProfiledPIDController<units::radian>{
|
||||
1.0, 0.0, 0.0,
|
||||
frc::TrapezoidProfile<units::radian>::Constraints{
|
||||
units::radians_per_second_t{2.0 * std::numbers::pi},
|
||||
units::radians_per_second_squared_t{std::numbers::pi}}}};
|
||||
|
||||
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
|
||||
|
||||
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
|
||||
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.0_mps, 4.0_mps_sq});
|
||||
|
||||
constexpr units::second_t kDt = 20_ms;
|
||||
auto totalTime = trajectory.TotalTime();
|
||||
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
|
||||
auto state = trajectory.Sample(kDt * i);
|
||||
auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
|
||||
|
||||
robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
|
||||
}
|
||||
|
||||
auto& endPose = trajectory.States().back().pose;
|
||||
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
|
||||
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
|
||||
EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
|
||||
kAngularTolerance);
|
||||
}
|
||||
|
||||
TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
|
||||
frc::HolonomicDriveController controller{
|
||||
frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
|
||||
frc::ProfiledPIDController<units::radian>{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radian>::Constraints{
|
||||
4_rad_per_s, 2_rad_per_s / 1_s}}};
|
||||
|
||||
frc::ChassisSpeeds speeds = controller.Calculate(
|
||||
frc::Pose2d{0_m, 0_m, 1.57_rad}, frc::Pose2d{}, 0_mps, 1.57_rad);
|
||||
|
||||
EXPECT_EQ(0, speeds.omega.value());
|
||||
}
|
||||
Reference in New Issue
Block a user