From e0e774abde64272ff344aecdaaaee0a747195528 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Fri, 1 Aug 2025 02:05:42 -0400 Subject: [PATCH] [commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119) --- .../command/MecanumControllerCommand.java | 576 ------------------ .../command/SwerveControllerCommand.java | 236 ------- .../frc2/command/MecanumControllerCommand.cpp | 224 ------- .../frc2/command/MecanumControllerCommand.h | 269 -------- .../frc2/command/SwerveControllerCommand.h | 270 -------- .../command/MecanumControllerCommandTest.java | 142 ----- .../command/SwerveControllerCommandTest.java | 142 ----- .../command/MecanumControllerCommandTest.cpp | 135 ---- .../command/SwerveControllerCommandTest.cpp | 113 ---- wpilibcExamples/example_projects.bzl | 4 - .../cpp/Constants.cpp | 22 - .../MecanumControllerCommand/cpp/Robot.cpp | 72 --- .../cpp/RobotContainer.cpp | 121 ---- .../cpp/subsystems/DriveSubsystem.cpp | 136 ----- .../include/Constants.h | 91 --- .../MecanumControllerCommand/include/Robot.h | 32 - .../include/RobotContainer.h | 48 -- .../include/subsystems/DriveSubsystem.h | 172 ------ .../SwerveControllerCommand/cpp/Constants.cpp | 12 - .../SwerveControllerCommand/cpp/Robot.cpp | 72 --- .../cpp/RobotContainer.cpp | 104 ---- .../cpp/subsystems/DriveSubsystem.cpp | 111 ---- .../cpp/subsystems/SwerveModule.cpp | 85 --- .../include/Constants.h | 116 ---- .../SwerveControllerCommand/include/Robot.h | 32 - .../include/RobotContainer.h | 43 -- .../include/subsystems/DriveSubsystem.h | 117 ---- .../include/subsystems/SwerveModule.h | 57 -- .../src/main/cpp/examples/examples.json | 36 -- .../cpp/MecanumControllerCommandTest.cpp | 64 -- .../MecanumControllerCommand/cpp/main.cpp | 16 - .../cpp/SwerveControllerCommandTest.cpp | 69 --- .../SwerveControllerCommand/cpp/main.cpp | 16 - wpilibjExamples/example_projects.bzl | 2 - .../wpi/first/wpilibj/examples/examples.json | 36 -- .../mecanumcontrollercommand/Constants.java | 87 --- .../mecanumcontrollercommand/Main.java | 25 - .../mecanumcontrollercommand/Robot.java | 100 --- .../RobotContainer.java | 130 ---- .../subsystems/DriveSubsystem.java | 239 -------- .../swervecontrollercommand/Constants.java | 116 ---- .../swervecontrollercommand/Main.java | 25 - .../swervecontrollercommand/Robot.java | 100 --- .../RobotContainer.java | 120 ---- .../subsystems/DriveSubsystem.java | 179 ------ .../subsystems/SwerveModule.java | 137 ----- .../controller/HolonomicDriveController.java | 168 ----- .../kinematics/MecanumDriveMotorVoltages.java | 53 -- .../frc/controller/HolonomicDriveController.h | 218 ------- .../HolonomicDriveControllerTest.java | 85 --- .../HolonomicDriveControllerTest.cpp | 67 -- 51 files changed, 5642 deletions(-) delete mode 100644 wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java delete mode 100644 wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java delete mode 100644 wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp delete mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h delete mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h delete mode 100644 wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java delete mode 100644 wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java delete mode 100644 wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp delete mode 100644 wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h delete mode 100644 wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h delete mode 100644 wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/MecanumControllerCommandTest.cpp delete mode 100644 wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/main.cpp delete mode 100644 wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/SwerveControllerCommandTest.cpp delete mode 100644 wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/main.cpp delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java delete mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java delete mode 100644 wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java delete mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java delete mode 100644 wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h delete mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java delete mode 100644 wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java deleted file mode 100644 index 65394cc0a6..0000000000 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ /dev/null @@ -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. - * - *

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. - * - *

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. - * - *

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 - */ -@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 m_pose; - private final SimpleMotorFeedforward m_feedforward; - private final MecanumDriveKinematics m_kinematics; - private final HolonomicDriveController m_controller; - private final Supplier 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 m_currentWheelSpeeds; - private final MecanumVoltagesConsumer m_outputDriveVoltages; - private final Consumer 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. - * - *

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 pose, - SimpleMotorFeedforward feedforward, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Supplier desiredRotation, - double maxWheelVelocity, - PIDController frontLeftController, - PIDController rearLeftController, - PIDController frontRightController, - PIDController rearRightController, - Supplier 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. - * - *

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 pose, - SimpleMotorFeedforward feedforward, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Supplier desiredRotation, - double maxWheelVelocityMetersPerSecond, - PIDController frontLeftController, - PIDController rearLeftController, - PIDController frontRightController, - PIDController rearRightController, - Supplier currentWheelSpeeds, - Consumer 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. - * - *

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. - * - *

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 pose, - SimpleMotorFeedforward feedforward, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - double maxWheelVelocityMetersPerSecond, - PIDController frontLeftController, - PIDController rearLeftController, - PIDController frontRightController, - PIDController rearRightController, - Supplier 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. - * - *

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. - * - *

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}. - */ - @Deprecated(since = "2025", forRemoval = true) - public MecanumControllerCommand( - Trajectory trajectory, - Supplier pose, - SimpleMotorFeedforward feedforward, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - double maxWheelVelocity, - PIDController frontLeftController, - PIDController rearLeftController, - PIDController frontRightController, - PIDController rearRightController, - Supplier currentWheelSpeeds, - Consumer 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. - * - *

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 pose, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Supplier desiredRotation, - double maxWheelVelocity, - Consumer 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. - * - *

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. - * - *

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 pose, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - double maxWheelVelocity, - Consumer 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); - } -} diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java deleted file mode 100644 index 06d63ebb2e..0000000000 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java +++ /dev/null @@ -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. - * - *

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. - * - *

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 - */ -public class SwerveControllerCommand extends Command { - private final Timer m_timer = new Timer(); - private final Trajectory m_trajectory; - private final Supplier m_pose; - private final SwerveDriveKinematics m_kinematics; - private final HolonomicDriveController m_controller; - private final Consumer m_outputModuleStates; - private final Supplier 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. - * - *

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 pose, - SwerveDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Supplier desiredRotation, - Consumer 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. - * - *

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. - * - *

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 pose, - SwerveDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - Consumer 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. - * - *

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. - * - *

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 pose, - SwerveDriveKinematics kinematics, - HolonomicDriveController controller, - Consumer 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. - * - *

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 pose, - SwerveDriveKinematics kinematics, - HolonomicDriveController controller, - Supplier desiredRotation, - Consumer 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()); - } -} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp deleted file mode 100644 index f08bab80e9..0000000000 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ /dev/null @@ -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 -#include - -#include -#include - -using namespace frc2; -using kv_unit = units::compound_unit>; - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - 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(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - 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(frontLeftController)), - m_rearLeftController( - std::make_unique(rearLeftController)), - m_frontRightController( - std::make_unique(frontRightController)), - m_rearRightController( - std::make_unique(rearRightController)), - m_currentWheelSpeeds(std::move(currentWheelSpeeds)), - m_outputVolts(std::move(output)), - m_usePID(true) { - AddRequirements(requirements); -} - -MecanumControllerCommand::MecanumControllerCommand( - frc::Trajectory trajectory, std::function pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - Requirements requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(0_V, units::unit_t{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 pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - Requirements requirements) - : m_trajectory(std::move(trajectory)), - m_pose(std::move(pose)), - m_feedforward(0_V, units::unit_t{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()); -} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h deleted file mode 100644 index c305054d1a..0000000000 --- a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h +++ /dev/null @@ -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 -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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. - * - *

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. - * - *

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. - * - *

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 { - 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. - * - *

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 pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - 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. - * - *

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. - * - *

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 pose, - frc::SimpleMotorFeedforward feedforward, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function currentWheelSpeeds, - frc::PIDController frontLeftController, - frc::PIDController rearLeftController, - frc::PIDController frontRightController, - frc::PIDController rearRightController, - std::function - 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. - * - *

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 pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - units::meters_per_second_t maxWheelVelocity, - std::function - 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. - * - *

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. - * - *

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 pose, - frc::MecanumDriveKinematics kinematics, frc::PIDController xController, - frc::PIDController yController, - frc::ProfiledPIDController thetaController, - units::meters_per_second_t maxWheelVelocity, - std::function - output, - Requirements requirements = {}); - - void Initialize() override; - - void Execute() override; - - void End(bool interrupted) override; - - bool IsFinished() override; - - private: - frc::Trajectory m_trajectory; - std::function m_pose; - frc::SimpleMotorFeedforward m_feedforward; - frc::MecanumDriveKinematics m_kinematics; - frc::HolonomicDriveController m_controller; - std::function m_desiredRotation; - const units::meters_per_second_t m_maxWheelVelocity; - std::unique_ptr m_frontLeftController; - std::unique_ptr m_rearLeftController; - std::unique_ptr m_frontRightController; - std::unique_ptr m_rearRightController; - std::function m_currentWheelSpeeds; - std::function - m_outputVel; - std::function - m_outputVolts; - - bool m_usePID; - frc::Timer m_timer; - frc::MecanumDriveWheelSpeeds m_prevSpeeds; - units::second_t m_prevTime; -}; -} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h deleted file mode 100644 index 75205e9928..0000000000 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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. - * - *

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. - * - *

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. - * - *

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 -class SwerveControllerCommand - : public CommandHelper> { - using voltsecondspermeter = - units::compound_unit>; - using voltsecondssquaredpermeter = - units::compound_unit, - units::inverse>; - - 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. - * - *

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 pose, - frc::SwerveDriveKinematics kinematics, - frc::PIDController xController, frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function desiredRotation, - std::function)> - 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. - * - *

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. - * - *

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 pose, - frc::SwerveDriveKinematics kinematics, - frc::PIDController xController, frc::PIDController yController, - frc::ProfiledPIDController thetaController, - std::function)> - 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. - * - *

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 pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function desiredRotation, - std::function)> - 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. - * - *

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. - * - *

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 pose, - frc::SwerveDriveKinematics kinematics, - frc::HolonomicDriveController controller, - std::function)> - 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 m_pose; - frc::SwerveDriveKinematics m_kinematics; - frc::HolonomicDriveController m_controller; - std::function)> - m_outputStates; - - std::function m_desiredRotation; - - frc::Timer m_timer; - units::second_t m_prevTime; - frc::Rotation2d m_finalRotation; -}; - -} // namespace frc2 diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java deleted file mode 100644 index 5f5f3731f5..0000000000 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java +++ /dev/null @@ -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(); - 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)); - } -} diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java deleted file mode 100644 index 2d91497f34..0000000000 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ /dev/null @@ -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(); - 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)); - } -} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp deleted file mode 100644 index 04af56a682..0000000000 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp +++ /dev/null @@ -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 -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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>>; - - 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 m_rotController{ - 1, 0, 0, - frc::TrapezoidProfile::Constraints{ - 9_rad_per_s, units::unit_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); -} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp deleted file mode 100644 index 82036514e4..0000000000 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ /dev/null @@ -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 -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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>>; - - protected: - frc::Timer m_timer; - frc::Rotation2d m_angle{0_rad}; - - wpi::array m_moduleStates{ - frc::SwerveModuleState{}, frc::SwerveModuleState{}, - frc::SwerveModuleState{}, frc::SwerveModuleState{}}; - - wpi::array m_modulePositions{ - frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, - frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}; - - frc::ProfiledPIDController m_rotController{ - 1, 0, 0, - frc::TrapezoidProfile::Constraints{ - 9_rad_per_s, units::unit_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); -} diff --git a/wpilibcExamples/example_projects.bzl b/wpilibcExamples/example_projects.bzl index 62f18905fe..76c832a726 100644 --- a/wpilibcExamples/example_projects.bzl +++ b/wpilibcExamples/example_projects.bzl @@ -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", ] diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp deleted file mode 100644 index 9fd38043e1..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp +++ /dev/null @@ -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::Constraints - kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration}; - -} // namespace AutoConstants diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp deleted file mode 100644 index 18be09af70..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp +++ /dev/null @@ -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 -#include - -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. - * - *

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(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp deleted file mode 100644 index 427da55401..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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(ks, kv, ka), - DriveConstants::kDriveKinematics, - - frc::PIDController{AutoConstants::kPXController, 0, 0}, - frc::PIDController{AutoConstants::kPYController, 0, 0}, - frc::ProfiledPIDController( - 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()); -} diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index 8a55821e71..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -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 -#include -#include - -#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); -} diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h deleted file mode 100644 index 098dfa2028..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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(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::Constraints - kThetaControllerConstraints; - -} // namespace AutoConstants - -namespace OIConstants { -inline constexpr int kDriverControllerPort = 0; -} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h deleted file mode 100644 index 2405694447..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h +++ /dev/null @@ -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 - -#include -#include - -#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 m_autonomousCommand; - - RobotContainer m_container; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h deleted file mode 100644 index 2437852294..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -#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(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h deleted file mode 100644 index 7b8ccae777..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -#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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp deleted file mode 100644 index 80fb825a04..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp +++ /dev/null @@ -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::Constraints - kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration}; - -} // namespace AutoConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp deleted file mode 100644 index 18be09af70..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp +++ /dev/null @@ -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 -#include - -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. - * - *

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(); -} -#endif diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp deleted file mode 100644 index 8cdada50d5..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 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()); -} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp deleted file mode 100644 index ae3cd7c8be..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#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 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); -} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp deleted file mode 100644 index be9917514b..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp +++ /dev/null @@ -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 - -#include - -#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(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h deleted file mode 100644 index b8c0f76940..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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(kEncoderCPR); - -inline constexpr double kTurningEncoderDistancePerPulse = - // Assumes the encoders are directly mounted on the wheel shafts - (std::numbers::pi * 2) / static_cast(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::Constraints - kThetaControllerConstraints; - -} // namespace AutoConstants - -namespace OIConstants { -inline constexpr int kDriverControllerPort = 0; -} // namespace OIConstants diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h deleted file mode 100644 index 2405694447..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h +++ /dev/null @@ -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 - -#include -#include - -#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 m_autonomousCommand; - - RobotContainer m_container; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h deleted file mode 100644 index cdb993eaa8..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -#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(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h deleted file mode 100644 index 9fc7386caf..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h deleted file mode 100644 index c7eac48d83..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include -#include - -#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 m_turningPIDController{ - ModuleConstants::kPModuleTurningController, - 0.0, - 0.0, - {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}}; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index ad4703d8fc..48f816a73d 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -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.", diff --git a/wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/MecanumControllerCommandTest.cpp b/wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/MecanumControllerCommandTest.cpp deleted file mode 100644 index 74d84f167f..0000000000 --- a/wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/MecanumControllerCommandTest.cpp +++ /dev/null @@ -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 - -#include -#include -#include -#include - -#include "Robot.h" - -class MecanumControllerCommandTest : public testing::Test { - Robot m_robot; - std::optional 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(); -} diff --git a/wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/main.cpp deleted file mode 100644 index 38d5cfa2fe..0000000000 --- a/wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/main.cpp +++ /dev/null @@ -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 -#include - -/** - * 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; -} diff --git a/wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/SwerveControllerCommandTest.cpp b/wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/SwerveControllerCommandTest.cpp deleted file mode 100644 index eb20804de0..0000000000 --- a/wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/SwerveControllerCommandTest.cpp +++ /dev/null @@ -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 -#include - -#include -#include -#include -#include - -#include "Robot.h" - -class SwerveControllerCommandTest : public testing::Test { - Robot m_robot; - std::optional 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(); -} diff --git a/wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/main.cpp deleted file mode 100644 index 38d5cfa2fe..0000000000 --- a/wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/main.cpp +++ /dev/null @@ -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 -#include - -/** - * 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; -} diff --git a/wpilibjExamples/example_projects.bzl b/wpilibjExamples/example_projects.bzl index e21ac8bc08..d432cc9468 100644 --- a/wpilibjExamples/example_projects.bzl +++ b/wpilibjExamples/example_projects.bzl @@ -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", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index b125ddb503..740d4aa64e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -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.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java deleted file mode 100644 index 8076219459..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java +++ /dev/null @@ -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. - * - *

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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java deleted file mode 100644 index 3398c216d7..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java +++ /dev/null @@ -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. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java deleted file mode 100644 index beec10031a..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java +++ /dev/null @@ -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. - * - *

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() {} -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java deleted file mode 100644 index cf8e088ddb..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java +++ /dev/null @@ -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))); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java deleted file mode 100644 index 0803adb206..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ /dev/null @@ -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(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java deleted file mode 100644 index 3a13b71779..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java +++ /dev/null @@ -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. - * - *

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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java deleted file mode 100644 index af62a0931e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java +++ /dev/null @@ -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. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java deleted file mode 100644 index 94f7eecf72..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java +++ /dev/null @@ -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. - * - *

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() {} -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java deleted file mode 100644 index b729de42bb..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ /dev/null @@ -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))); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java deleted file mode 100644 index 865c88904e..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java deleted file mode 100644 index 0ee79e7d02..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java +++ /dev/null @@ -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(); - } -} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java deleted file mode 100644 index c18cfebedc..0000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ /dev/null @@ -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. - * - *

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; - } -} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java deleted file mode 100644 index d9458049bd..0000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java +++ /dev/null @@ -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); - } -} diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h deleted file mode 100644 index b823c48450..0000000000 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ /dev/null @@ -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 - -#include - -#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 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& 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& 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 m_thetaController; - - bool m_firstRun = true; -}; -} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java deleted file mode 100644 index 870a9bc327..0000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java +++ /dev/null @@ -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 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 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); - } -} diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp deleted file mode 100644 index 746a7d08af..0000000000 --- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp +++ /dev/null @@ -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 - -#include - -#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{ - 1.0, 0.0, 0.0, - frc::TrapezoidProfile::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{ - 1, 0, 0, - frc::TrapezoidProfile::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()); -}