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

[command] Modify swerve and mecanum commands to use new controller

Browse Source
This commit is contained in:
Prateek Machiraju
2020-07-12 22:04:21 -04:00
committed by Peter Johnson
parent 5ca2702083
commit aba035eb3d
6 changed files with 761 additions and 292 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,7 @@
#include <initializer_list>
#include <memory>
#include <frc/controller/HolonomicDriveController.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/geometry/Pose2d.h>
@@ -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.
*
* <p>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<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc2::PIDController xController, frc2::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
std::initializer_list<Subsystem*> requirements);
/**
* Constructs a new SwerveControllerCommand that when executed will follow the
* provided trajectory. This command will not return output voltages but
* rather raw module states from the position controllers which need to be put
* into a velocity PID.
*
* <p>Note: The controllers will *not* set the outputVolts to zero upon
* completion of the path- this is left to the user, since it is not
* appropriate for paths with nonstationary endstates.
*
* <p>Note 2: The final rotation of the robot will be set to the rotation of
* the final pose in the trajectory. The robot will not follow the rotations
* from the poses at each timestep. If alternate rotation behavior is desired,
* the other constructor with a supplier for rotation should be used.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose,
@@ -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.
*
* <p>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<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc2::PIDController xController, frc2::PIDController yController,
frc::ProfiledPIDController<units::radians> thetaController,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
wpi::ArrayRef<Subsystem*> requirements = {});
/**
* Constructs a new SwerveControllerCommand that when executed will follow the
* provided trajectory. This command will not return output voltages but
* rather raw module states from the position controllers which need to be put
* into a velocity PID.
*
* <p>Note: The controllers will *not* set the outputVolts to zero upon
* completion of the path- this is left to the user, since it is not
* appropriate for paths with nonstationary endstates.
*
* <p>Note 2: The final rotation of the robot will be set to the rotation of
* the final pose in the trajectory. The robot will not follow the rotations
* from the poses at each timestep. If alternate rotation behavior is desired,
* the other constructor with a supplier for rotation should be used.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose,
@@ -144,15 +222,15 @@ class SwerveControllerCommand
frc::Trajectory m_trajectory;
std::function<frc::Pose2d()> m_pose;
frc::SwerveDriveKinematics<NumModules> m_kinematics;
std::unique_ptr<frc2::PIDController> m_xController;
std::unique_ptr<frc2::PIDController> m_yController;
std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
frc::HolonomicDriveController m_controller;
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
m_outputStates;
std::function<frc::Rotation2d()> m_desiredRotation;
frc2::Timer m_timer;
units::second_t m_prevTime;
frc::Pose2d m_finalPose;
frc::Rotation2d m_finalRotation;
};
} // namespace frc2