Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

82
wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
View File

@@ -17,16 +17,51 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
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)
: m_trajectory(trajectory),
m_pose(pose),
m_kinematics(kinematics),
m_xController(std::make_unique<frc2::PIDController>(xController)),
m_yController(std::make_unique<frc2::PIDController>(yController)),
m_thetaController(
std::make_unique<frc::ProfiledPIDController<units::radians>>(
thetaController)),
m_controller(xController, yController, thetaController),
m_desiredRotation(desiredRotation),
m_outputStates(output) {
this->AddRequirements(requirements);
}
template <size_t NumModules>
SwerveControllerCommand<NumModules>::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<void(std::array<frc::SwerveModuleState, NumModules>)> output,
std::initializer_list<Subsystem*> 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 <size_t NumModules>
SwerveControllerCommand<NumModules>::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)
: 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<NumModules>::SwerveControllerCommand(
: m_trajectory(trajectory),
m_pose(pose),
m_kinematics(kinematics),
m_xController(std::make_unique<frc2::PIDController>(xController)),
m_yController(std::make_unique<frc2::PIDController>(yController)),
m_thetaController(
std::make_unique<frc::ProfiledPIDController<units::radians>>(
thetaController)),
m_controller(xController, yController, thetaController),
m_outputStates(output) {
this->AddRequirements(requirements);
m_desiredRotation = [&] {
return m_trajectory.States().back().pose.Rotation();
};
}
template <size_t NumModules>
void SwerveControllerCommand<NumModules>::Initialize() {
m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
m_timer.Reset();
m_timer.Start();
}
@@ -62,34 +94,10 @@ void SwerveControllerCommand<NumModules>::Initialize() {
template <size_t NumModules>
void SwerveControllerCommand<NumModules>::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<double>()),
(m_desiredPose.X().template to<double>())));
auto targetYVel = units::meters_per_second_t(
m_yController->Calculate((m_pose().Y().template to<double>()),
(m_desiredPose.Y().template to<double>())));
// 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);