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 index b112a523e8..9105f7aa44 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -11,10 +11,12 @@ import java.util.function.Consumer; import java.util.function.Supplier; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.HolonomicDriveController; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics; import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages; @@ -45,18 +47,13 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; @SuppressWarnings({"PMD.TooManyFields", "MemberName"}) public class MecanumControllerCommand extends CommandBase { private final Timer m_timer = new Timer(); - private MecanumDriveWheelSpeeds m_prevSpeeds; - private double m_prevTime; - private Pose2d m_finalPose; private final boolean m_usePID; - private final Trajectory m_trajectory; private final Supplier m_pose; private final SimpleMotorFeedforward m_feedforward; private final MecanumDriveKinematics m_kinematics; - private final PIDController m_xController; - private final PIDController m_yController; - private final ProfiledPIDController m_thetaController; + private final HolonomicDriveController m_controller; + private final Supplier m_desiredRotation; private final double m_maxWheelVelocityMetersPerSecond; private final PIDController m_frontLeftController; private final PIDController m_rearLeftController; @@ -65,6 +62,8 @@ public class MecanumControllerCommand extends CommandBase { private final Supplier m_currentWheelSpeeds; private final Consumer m_outputDriveVoltages; private final Consumer m_outputWheelSpeeds; + private MecanumDriveWheelSpeeds m_prevSpeeds; + private double m_prevTime; /** * Constructs a new MecanumControllerCommand that when executed will follow the provided @@ -74,8 +73,105 @@ public class MecanumControllerCommand extends CommandBase { *

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 rotation controller will calculate the rotation based on the final pose in the - * trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. This + * is sampled at each time step. + * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the current wheel speeds. + * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing + * the output motor voltages. + * @param requirements The subsystems to require. + */ + + @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"}) + public MecanumControllerCommand(Trajectory trajectory, + Supplier 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) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", + "MecanumControllerCommand"); + m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand"); + m_feedforward = requireNonNullParam(feedforward, "feedforward", + "MecanumControllerCommand"); + m_kinematics = requireNonNullParam(kinematics, "kinematics", + "MecanumControllerCommand"); + + m_controller = new HolonomicDriveController( + requireNonNullParam(xController, + "xController", "SwerveControllerCommand"), + requireNonNullParam(yController, + "xController", "SwerveControllerCommand"), + requireNonNullParam(thetaController, + "thetaController", "SwerveControllerCommand") + ); + + m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation", + "MecanumControllerCommand"); + + m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond, + "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand"); + + m_frontLeftController = requireNonNullParam(frontLeftController, + "frontLeftController", "MecanumControllerCommand"); + m_rearLeftController = requireNonNullParam(rearLeftController, + "rearLeftController", "MecanumControllerCommand"); + m_frontRightController = requireNonNullParam(frontRightController, + "frontRightController", "MecanumControllerCommand"); + m_rearRightController = requireNonNullParam(rearRightController, + "rearRightController", "MecanumControllerCommand"); + + m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds, + "currentWheelSpeeds", "MecanumControllerCommand"); + + m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages, + "outputDriveVoltages", "MecanumControllerCommand"); + + m_outputWheelSpeeds = null; + + m_usePID = true; + + addRequirements(requirements); + } + + /** + * Constructs a new MecanumControllerCommand that when executed will follow the provided + * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to + * 12 as a voltage output to the motor. + * + *

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 @@ -102,58 +198,100 @@ public class MecanumControllerCommand extends CommandBase { @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"}) public MecanumControllerCommand(Trajectory trajectory, - Supplier pose, - SimpleMotorFeedforward feedforward, - MecanumDriveKinematics kinematics, + Supplier pose, + SimpleMotorFeedforward feedforward, + MecanumDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + double maxWheelVelocityMetersPerSecond, + PIDController frontLeftController, + PIDController rearLeftController, + PIDController frontRightController, + PIDController rearRightController, + Supplier currentWheelSpeeds, + Consumer outputDriveVoltages, + Subsystem... requirements) { - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, + this(trajectory, pose, feedforward, kinematics, xController, yController, thetaController, + () -> trajectory.getStates().get(trajectory.getStates().size() - 1) + .poseMeters.getRotation(), + maxWheelVelocityMetersPerSecond, frontLeftController, rearLeftController, + frontRightController, rearRightController, currentWheelSpeeds, outputDriveVoltages, + requirements); + } - double maxWheelVelocityMetersPerSecond, + /** + * Constructs a new MecanumControllerCommand that when executed will follow the provided + * trajectory. The user should implement a velocity PID on the desired output wheel velocities. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path - + * this is left to the user, since it is not appropriate for paths with non-stationary end-states. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. This + * is sampled at each time step. + * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the output wheel speeds. + * @param requirements The subsystems to require. + */ - PIDController frontLeftController, - PIDController rearLeftController, - PIDController frontRightController, - PIDController rearRightController, - - Supplier currentWheelSpeeds, - - Consumer outputDriveVoltages, - Subsystem... requirements) { - m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand"); + @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"}) + public MecanumControllerCommand(Trajectory trajectory, + Supplier pose, + MecanumDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + Supplier desiredRotation, + double maxWheelVelocityMetersPerSecond, + Consumer outputWheelSpeeds, + Subsystem... requirements) { + m_trajectory = requireNonNullParam(trajectory, "trajectory", + "MecanumControllerCommand"); m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand"); - m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand"); - m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand"); + m_feedforward = new SimpleMotorFeedforward(0, 0, 0); + m_kinematics = requireNonNullParam(kinematics, + "kinematics", "MecanumControllerCommand"); - m_xController = requireNonNullParam(xController, "xController", - "MecanumControllerCommand"); - m_yController = requireNonNullParam(yController, "yController", - "MecanumControllerCommand"); - m_thetaController = requireNonNullParam(thetaController, "thetaController", - "MecanumControllerCommand"); + m_controller = new HolonomicDriveController( + requireNonNullParam(xController, + "xController", "SwerveControllerCommand"), + requireNonNullParam(yController, + "xController", "SwerveControllerCommand"), + requireNonNullParam(thetaController, + "thetaController", "SwerveControllerCommand") + ); + + m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation", + "MecanumControllerCommand"); m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond, - "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand"); + "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand"); - m_frontLeftController = requireNonNullParam(frontLeftController, - "frontLeftController", "MecanumControllerCommand"); - m_rearLeftController = requireNonNullParam(rearLeftController, - "rearLeftController", "MecanumControllerCommand"); - m_frontRightController = requireNonNullParam(frontRightController, - "frontRightController", "MecanumControllerCommand"); - m_rearRightController = requireNonNullParam(rearRightController, - "rearRightController", "MecanumControllerCommand"); + m_frontLeftController = null; + m_rearLeftController = null; + m_frontRightController = null; + m_rearRightController = null; - m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds, - "currentWheelSpeeds", "MecanumControllerCommand"); + m_currentWheelSpeeds = null; - m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages, - "outputDriveVoltages", "MecanumControllerCommand"); + m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds, + "outputWheelSpeeds", "MecanumControllerCommand"); - m_outputWheelSpeeds = null; + m_outputDriveVoltages = null; - m_usePID = true; + m_usePID = false; addRequirements(requirements); } @@ -165,8 +303,10 @@ public class MecanumControllerCommand extends CommandBase { *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path - * this is left to the user, since it is not appropriate for paths with non-stationary end-states. * - *

Note2: The rotation controller will calculate the rotation based on the final pose - * in the trajectory, not the poses at each time step. + *

Note 2: The final rotation of the robot will be set to the rotation of + * the final pose in the trajectory. The robot will not follow the rotations + * from the poses at each timestep. If alternate rotation behavior is desired, + * the other constructor with a supplier for rotation should be used. * * @param trajectory The trajectory to follow. * @param pose A function that supplies the robot pose - use one of @@ -186,63 +326,31 @@ public class MecanumControllerCommand extends CommandBase { @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"}) public MecanumControllerCommand(Trajectory trajectory, - Supplier pose, - MecanumDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - - double maxWheelVelocityMetersPerSecond, - - 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_xController = requireNonNullParam(xController, - "xController", "MecanumControllerCommand"); - m_yController = requireNonNullParam(yController, - "xController", "MecanumControllerCommand"); - m_thetaController = requireNonNullParam(thetaController, - "thetaController", "MecanumControllerCommand"); - - m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond, - "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand"); - - m_frontLeftController = null; - m_rearLeftController = null; - m_frontRightController = null; - m_rearRightController = null; - - m_currentWheelSpeeds = null; - - m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds, - "outputWheelSpeeds", "MecanumControllerCommand"); - - m_outputDriveVoltages = null; - - m_usePID = false; - - addRequirements(requirements); + Supplier pose, + MecanumDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + double maxWheelVelocityMetersPerSecond, + Consumer outputWheelSpeeds, + Subsystem... requirements) { + this(trajectory, pose, kinematics, xController, yController, thetaController, + () -> trajectory.getStates().get(trajectory.getStates().size() - 1) + .poseMeters.getRotation(), + maxWheelVelocityMetersPerSecond, outputWheelSpeeds, requirements); } @Override public void initialize() { var initialState = m_trajectory.sample(0); - // Sample final pose to get robot rotation - m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters; - var initialXVelocity = initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos(); var initialYVelocity = initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin(); m_prevSpeeds = m_kinematics.toWheelSpeeds( - new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); + new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); m_timer.reset(); m_timer.start(); @@ -255,31 +363,9 @@ public class MecanumControllerCommand extends CommandBase { double dt = curTime - m_prevTime; var desiredState = m_trajectory.sample(curTime); - var desiredPose = desiredState.poseMeters; - - var poseError = desiredPose.relativeTo(m_pose.get()); - - double targetXVel = m_xController.calculate( - m_pose.get().getX(), - desiredPose.getX()); - - double targetYVel = m_yController.calculate( - m_pose.get().getY(), - desiredPose.getY()); - - // The robot will go to the desired rotation of the final pose in the trajectory, - // not following the poses at individual states. - double targetAngularVel = m_thetaController.calculate( - m_pose.get().getRotation().getRadians(), - m_finalPose.getRotation().getRadians()); - - double vRef = desiredState.velocityMetersPerSecond; - - targetXVel += vRef * poseError.getRotation().getCos(); - targetYVel += vRef * poseError.getRotation().getSin(); - - var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel); + var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState, + m_desiredRotation.get()); var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds); targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond); @@ -287,7 +373,7 @@ public class MecanumControllerCommand extends CommandBase { var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; - var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; + var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; double frontLeftOutput; double rearLeftOutput; @@ -296,32 +382,32 @@ public class MecanumControllerCommand extends CommandBase { if (m_usePID) { final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint, - (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt); + (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt); final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint, - (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt); + (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt); final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint, - (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt); + (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt); final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint, - (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt); + (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt); frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate( - m_currentWheelSpeeds.get().frontLeftMetersPerSecond, - frontLeftSpeedSetpoint); + m_currentWheelSpeeds.get().frontLeftMetersPerSecond, + frontLeftSpeedSetpoint); rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate( - m_currentWheelSpeeds.get().rearLeftMetersPerSecond, - rearLeftSpeedSetpoint); + m_currentWheelSpeeds.get().rearLeftMetersPerSecond, + rearLeftSpeedSetpoint); frontRightOutput = frontRightFeedforward + m_frontRightController.calculate( - m_currentWheelSpeeds.get().frontRightMetersPerSecond, - frontRightSpeedSetpoint); + m_currentWheelSpeeds.get().frontRightMetersPerSecond, + frontRightSpeedSetpoint); rearRightOutput = rearRightFeedforward + m_rearRightController.calculate( - m_currentWheelSpeeds.get().rearRightMetersPerSecond, - rearRightSpeedSetpoint); + m_currentWheelSpeeds.get().rearRightMetersPerSecond, + rearRightSpeedSetpoint); m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages( frontLeftOutput, 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 index 7ea198392a..c0303b96db 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java @@ -7,15 +7,15 @@ package edu.wpi.first.wpilibj2.command; - import java.util.function.Consumer; import java.util.function.Supplier; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.HolonomicDriveController; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.trajectory.Trajectory; @@ -38,15 +38,12 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; @SuppressWarnings("MemberName") public class SwerveControllerCommand extends CommandBase { private final Timer m_timer = new Timer(); - private Pose2d m_finalPose; - private final Trajectory m_trajectory; private final Supplier m_pose; private final SwerveDriveKinematics m_kinematics; - private final PIDController m_xController; - private final PIDController m_yController; - private final ProfiledPIDController m_thetaController; + private final HolonomicDriveController m_controller; private final Consumer m_outputModuleStates; + private final Supplier m_desiredRotation; /** * Constructs a new SwerveControllerCommand that when executed will follow the provided @@ -56,8 +53,66 @@ public class SwerveControllerCommand extends CommandBase { *

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 rotation controller will calculate the rotation based on the final pose - * in the trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of + * the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the drivetrain should be facing. This + * is sampled at each time step. + * @param outputModuleStates The raw output module states from the + * position controllers. + * @param requirements The subsystems to require. + */ + @SuppressWarnings("ParameterName") + public SwerveControllerCommand(Trajectory trajectory, + Supplier pose, + SwerveDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + 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 = new HolonomicDriveController( + requireNonNullParam(xController, + "xController", "SwerveControllerCommand"), + requireNonNullParam(yController, + "xController", "SwerveControllerCommand"), + requireNonNullParam(thetaController, + "thetaController", "SwerveControllerCommand") + ); + + m_outputModuleStates = requireNonNullParam(outputModuleStates, + "frontLeftOutput", "SwerveControllerCommand"); + + m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation", + "SwerveControllerCommand"); + + addRequirements(requirements); + } + + /** + * Constructs a new SwerveControllerCommand that when executed will follow the provided + * trajectory. This command will not return output voltages but rather raw module states from the + * position controllers which need to be put into a velocity PID. + * + *

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 @@ -73,38 +128,23 @@ public class SwerveControllerCommand extends CommandBase { * position controllers. * @param requirements The subsystems to require. */ - @SuppressWarnings("ParameterName") public SwerveControllerCommand(Trajectory trajectory, - Supplier pose, - SwerveDriveKinematics kinematics, - PIDController xController, - PIDController yController, - ProfiledPIDController thetaController, - - Consumer outputModuleStates, - Subsystem... requirements) { - m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand"); - m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand"); - m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand"); - - m_xController = requireNonNullParam(xController, - "xController", "SwerveControllerCommand"); - m_yController = requireNonNullParam(yController, - "xController", "SwerveControllerCommand"); - m_thetaController = requireNonNullParam(thetaController, - "thetaController", "SwerveControllerCommand"); - - m_outputModuleStates = requireNonNullParam(outputModuleStates, - "frontLeftOutput", "SwerveControllerCommand"); - addRequirements(requirements); + Supplier 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) + .poseMeters.getRotation(), + outputModuleStates, requirements); } @Override public void initialize() { - // Sample final pose to get robot rotation - m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters; - m_timer.reset(); m_timer.start(); } @@ -113,37 +153,13 @@ public class SwerveControllerCommand extends CommandBase { @SuppressWarnings("LocalVariableName") public void execute() { double curTime = m_timer.get(); - var desiredState = m_trajectory.sample(curTime); - var desiredPose = desiredState.poseMeters; - - var poseError = desiredPose.relativeTo(m_pose.get()); - - double targetXVel = m_xController.calculate( - m_pose.get().getX(), - desiredPose.getX()); - - double targetYVel = m_yController.calculate( - m_pose.get().getY(), - desiredPose.getY()); - - // The robot will go to the desired rotation of the final pose in the trajectory, - // not following the poses at individual states. - double targetAngularVel = m_thetaController.calculate( - m_pose.get().getRotation().getRadians(), - m_finalPose.getRotation().getRadians()); - - double vRef = desiredState.velocityMetersPerSecond; - - targetXVel += vRef * poseError.getRotation().getCos(); - targetYVel += vRef * poseError.getRotation().getSin(); - - var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel); + var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState, + m_desiredRotation.get()); var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds); m_outputModuleStates.accept(targetModuleStates); - } @Override diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index fbc3c7355d..eb32853fbc 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -16,6 +16,7 @@ MecanumControllerCommand::MecanumControllerCommand( frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController, + std::function desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function currentWheelSpeeds, frc2::PIDController frontLeftController, @@ -30,11 +31,85 @@ MecanumControllerCommand::MecanumControllerCommand( m_pose(pose), m_feedforward(feedforward), m_kinematics(kinematics), - m_xController(std::make_unique(xController)), - m_yController(std::make_unique(yController)), - m_thetaController( - std::make_unique>( - thetaController)), + m_controller(xController, yController, thetaController), + m_desiredRotation(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(currentWheelSpeeds), + m_outputVolts(output), + m_usePID(true) { + AddRequirements(requirements); +} + +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_feedforward(feedforward), + m_kinematics(kinematics), + m_controller(xController, yController, thetaController), + m_maxWheelVelocity(maxWheelVelocity), + m_frontLeftController( + std::make_unique(frontLeftController)), + m_rearLeftController( + std::make_unique(rearLeftController)), + m_frontRightController( + std::make_unique(frontRightController)), + m_rearRightController( + std::make_unique(rearRightController)), + m_currentWheelSpeeds(currentWheelSpeeds), + m_outputVolts(output), + m_usePID(true) { + AddRequirements(requirements); + m_desiredRotation = [&] { + return m_trajectory.States().back().pose.Rotation(); + }; +} + +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_feedforward(feedforward), + m_kinematics(kinematics), + m_controller(xController, yController, thetaController), + m_desiredRotation(desiredRotation), m_maxWheelVelocity(maxWheelVelocity), m_frontLeftController( std::make_unique(frontLeftController)), @@ -70,11 +145,7 @@ MecanumControllerCommand::MecanumControllerCommand( m_pose(pose), m_feedforward(feedforward), m_kinematics(kinematics), - m_xController(std::make_unique(xController)), - m_yController(std::make_unique(yController)), - m_thetaController( - std::make_unique>( - thetaController)), + m_controller(xController, yController, thetaController), m_maxWheelVelocity(maxWheelVelocity), m_frontLeftController( std::make_unique(frontLeftController)), @@ -88,6 +159,31 @@ MecanumControllerCommand::MecanumControllerCommand( m_outputVolts(output), m_usePID(true) { AddRequirements(requirements); + m_desiredRotation = [&] { + return m_trajectory.States().back().pose.Rotation(); + }; +} + +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_controller(xController, yController, thetaController), + m_desiredRotation(desiredRotation), + m_maxWheelVelocity(maxWheelVelocity), + m_outputVel(output), + m_usePID(false) { + AddRequirements(requirements); } MecanumControllerCommand::MecanumControllerCommand( @@ -103,11 +199,32 @@ MecanumControllerCommand::MecanumControllerCommand( : m_trajectory(trajectory), m_pose(pose), m_kinematics(kinematics), - m_xController(std::make_unique(xController)), - m_yController(std::make_unique(yController)), - m_thetaController( - std::make_unique>( - thetaController)), + m_controller(xController, yController, thetaController), + m_maxWheelVelocity(maxWheelVelocity), + m_outputVel(output), + m_usePID(false) { + AddRequirements(requirements); + m_desiredRotation = [&] { + return m_trajectory.States().back().pose.Rotation(); + }; +} + +MecanumControllerCommand::MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_controller(xController, yController, thetaController), + m_desiredRotation(desiredRotation), m_maxWheelVelocity(maxWheelVelocity), m_outputVel(output), m_usePID(false) { @@ -127,15 +244,14 @@ MecanumControllerCommand::MecanumControllerCommand( : m_trajectory(trajectory), m_pose(pose), m_kinematics(kinematics), - m_xController(std::make_unique(xController)), - m_yController(std::make_unique(yController)), - m_thetaController( - std::make_unique>( - thetaController)), + m_controller(xController, yController, thetaController), m_maxWheelVelocity(maxWheelVelocity), m_outputVel(output), m_usePID(false) { AddRequirements(requirements); + m_desiredRotation = [&] { + return m_trajectory.States().back().pose.Rotation(); + }; } void MecanumControllerCommand::Initialize() { @@ -165,29 +281,9 @@ void MecanumControllerCommand::Execute() { auto dt = curTime - m_prevTime; auto m_desiredState = m_trajectory.Sample(curTime); - auto m_desiredPose = m_desiredState.pose; - - auto m_poseError = m_desiredPose.RelativeTo(m_pose()); - - auto targetXVel = meters_per_second_t(m_xController->Calculate( - (m_pose().X().to()), (m_desiredPose.X().to()))); - auto targetYVel = meters_per_second_t(m_yController->Calculate( - (m_pose().Y().to()), (m_desiredPose.Y().to()))); - - // Profiled PID Controller only takes meters as setpoint and measurement - // The robot will go to the desired rotation of the final pose in the - // trajectory, not following the poses at individual states. - auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate( - m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians())); - - auto vRef = m_desiredState.velocity; - - targetXVel += vRef * m_poseError.Rotation().Cos(); - targetYVel += vRef * m_poseError.Rotation().Sin(); auto targetChassisSpeeds = - frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel}; - + m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation()); auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds); targetWheelSpeeds.Normalize(m_maxWheelVelocity); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h index baae261411..87b16ee929 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -63,8 +64,61 @@ class MecanumControllerCommand * completion of the path this is left to the user, since it is not * appropriate for paths with nonstationary endstates. * - *

Note 2: The rotation controller will calculate the rotation based on the - * final pose in the trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. + * This is sampled at each time step. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the current wheel speeds. + * @param output The output of the velocity PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + std::initializer_list 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, @@ -114,8 +168,61 @@ class MecanumControllerCommand * completion of the path this is left to the user, since it is not * appropriate for paths with nonstationary endstates. * - *

Note 2: The rotation controller will calculate the rotation based on the - * final pose in the trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param feedforward The feedforward to use for the drivetrain. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. + * This is sampled at each time step. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param frontLeftController The front left wheel velocity PID. + * @param rearLeftController The rear left wheel velocity PID. + * @param frontRightController The front right wheel velocity PID. + * @param rearRightController The rear right wheel velocity PID. + * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing + * the current wheel speeds. + * @param output The output of the velocity PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SimpleMotorFeedforward feedforward, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function currentWheelSpeeds, + frc2::PIDController frontLeftController, + frc2::PIDController rearLeftController, + frc2::PIDController frontRightController, + frc2::PIDController rearRightController, + std::function + output, + wpi::ArrayRef 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, @@ -164,8 +271,48 @@ class MecanumControllerCommand * completion of the path - this is left to the user, since it is not * appropriate for paths with non-stationary end-states. * - *

Note2: The rotation controller will calculate the rotation based on the - * final pose in the trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one + * of the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. + * This is sampled at each time step. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param output The output of the position PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + std::initializer_list 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 non-stationary 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 @@ -202,8 +349,48 @@ class MecanumControllerCommand * completion of the path - this is left to the user, since it is not * appropriate for paths with non-stationary end-states. * - *

Note2: The rotation controller will calculate the rotation based on the - * final pose in the trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one + * of the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the robot should be facing. + * This is sampled at every time step. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. + * @param output The output of the position PIDs. + * @param requirements The subsystems to require. + */ + MecanumControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, + frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + units::meters_per_second_t maxWheelVelocity, + std::function + output, + wpi::ArrayRef 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 non-stationary end-states. + * + *

Note2: The final rotation of the robot will be set to the rotation of + * the final pose in the trajectory. The robot will not follow the rotations + * from the poses at each timestep. If alternate rotation behavior is desired, + * the other constructor with a supplier for rotation should be used. * * @param trajectory The trajectory to follow. * @param pose A function that supplies the robot pose - use one @@ -244,9 +431,8 @@ class MecanumControllerCommand std::function m_pose; frc::SimpleMotorFeedforward m_feedforward; frc::MecanumDriveKinematics m_kinematics; - std::unique_ptr m_xController; - std::unique_ptr m_yController; - std::unique_ptr> m_thetaController; + 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; @@ -264,6 +450,5 @@ class MecanumControllerCommand frc2::Timer m_timer; frc::MecanumDriveWheelSpeeds m_prevSpeeds; units::second_t m_prevTime; - frc::Pose2d m_finalPose; }; } // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h index 30a8d337d5..6036ce895e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -70,8 +71,46 @@ class SwerveControllerCommand * completion of the path- this is left to the user, since it is not * appropriate for paths with nonstationary endstates. * - *

Note 2: The rotation controller will calculate the rotation based on the - * final pose in the trajectory, not the poses at each time step. + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the drivetrain should be + * facing. This is sampled at each time step. + * @param output The raw output module states from the + * position controllers. + * @param requirements The subsystems to require. + */ + SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + std::function)> + output, + std::initializer_list 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, @@ -106,8 +145,47 @@ class SwerveControllerCommand * completion of the path- this is left to the user, since it is not * appropriate for paths with nonstationary endstates. * - *

Note 2: The rotation controller will calculate the rotation based on the - * final pose in the trajectory, not the poses at each time step. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose, + * provided by the odometry class. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller + * for the robot's x position. + * @param yController The Trajectory Tracker PID controller + * for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller + * for angle for the robot. + * @param desiredRotation The angle that the drivetrain should be + * facing. This is sampled at each time step. + * @param output The raw output module states from the + * position controllers. + * @param requirements The subsystems to require. + */ + SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + std::function)> + output, + wpi::ArrayRef 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, @@ -144,15 +222,15 @@ class SwerveControllerCommand frc::Trajectory m_trajectory; std::function m_pose; frc::SwerveDriveKinematics m_kinematics; - std::unique_ptr m_xController; - std::unique_ptr m_yController; - std::unique_ptr> m_thetaController; + frc::HolonomicDriveController m_controller; std::function)> m_outputStates; + std::function m_desiredRotation; + frc2::Timer m_timer; units::second_t m_prevTime; - frc::Pose2d m_finalPose; + frc::Rotation2d m_finalRotation; }; } // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc index 9c047ad482..616244ec79 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -17,16 +17,51 @@ SwerveControllerCommand::SwerveControllerCommand( frc::SwerveDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController thetaController, + std::function desiredRotation, std::function)> output, std::initializer_list requirements) : m_trajectory(trajectory), m_pose(pose), m_kinematics(kinematics), - m_xController(std::make_unique(xController)), - m_yController(std::make_unique(yController)), - m_thetaController( - std::make_unique>( - thetaController)), + m_controller(xController, yController, thetaController), + m_desiredRotation(desiredRotation), + m_outputStates(output) { + this->AddRequirements(requirements); +} + +template +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function)> output, + std::initializer_list requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_controller(xController, yController, thetaController), + m_outputStates(output) { + this->AddRequirements(requirements); + m_desiredRotation = [&] { + return m_trajectory.States().back().pose.Rotation(); + }; +} + +template +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc2::PIDController xController, frc2::PIDController yController, + frc::ProfiledPIDController thetaController, + std::function desiredRotation, + std::function)> output, + wpi::ArrayRef requirements) + : m_trajectory(trajectory), + m_pose(pose), + m_kinematics(kinematics), + m_controller(xController, yController, thetaController), + m_desiredRotation(desiredRotation), m_outputStates(output) { this->AddRequirements(requirements); } @@ -42,19 +77,16 @@ SwerveControllerCommand::SwerveControllerCommand( : m_trajectory(trajectory), m_pose(pose), m_kinematics(kinematics), - m_xController(std::make_unique(xController)), - m_yController(std::make_unique(yController)), - m_thetaController( - std::make_unique>( - thetaController)), + m_controller(xController, yController, thetaController), m_outputStates(output) { this->AddRequirements(requirements); + m_desiredRotation = [&] { + return m_trajectory.States().back().pose.Rotation(); + }; } template void SwerveControllerCommand::Initialize() { - m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose; - m_timer.Reset(); m_timer.Start(); } @@ -62,34 +94,10 @@ void SwerveControllerCommand::Initialize() { template void SwerveControllerCommand::Execute() { auto curTime = units::second_t(m_timer.Get()); - auto m_desiredState = m_trajectory.Sample(curTime); - auto m_desiredPose = m_desiredState.pose; - - auto m_poseError = m_desiredPose.RelativeTo(m_pose()); - - auto targetXVel = units::meters_per_second_t( - m_xController->Calculate((m_pose().X().template to()), - (m_desiredPose.X().template to()))); - auto targetYVel = units::meters_per_second_t( - m_yController->Calculate((m_pose().Y().template to()), - (m_desiredPose.Y().template to()))); - - // Profiled PID Controller only takes meters as setpoint and measurement - // The robot will go to the desired rotation of the final pose in the - // trajectory, not following the poses at individual states. - auto targetAngularVel = - units::radians_per_second_t(m_thetaController->Calculate( - m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians())); - - auto vRef = m_desiredState.velocity; - - targetXVel += vRef * m_poseError.Rotation().Cos(); - targetYVel += vRef * m_poseError.Rotation().Sin(); auto targetChassisSpeeds = - frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel}; - + m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation()); auto targetModuleStates = m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);