diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index 365ef81..d366398 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -109,6 +109,40 @@ SpeakerSide SwerveSubsystem::GetSpeakerSideFromPosition() { } } +frc2::CommandPtr SwerveSubsystem::RotateToAngle( + std::function xVel, + std::function yVel, + std::function goalAngle, + std::function override) { + return frc2::cmd::Sequence( + frc2::cmd::RunOnce([this] { + thetaController.EnableContinuousInput( + units::radian_t{-std::numbers::pi}, + units::radian_t{std::numbers::pi}); + thetaController.SetTolerance( + consts::swerve::pathplanning::rotationalPIDTolerance, + consts::swerve::pathplanning::rotationalVelPIDTolerance); + }, {this}), + frc2::cmd::Run([this, xVel, yVel, goalAngle] { + + units::radian_t goal = goalAngle(); + frc::Pose2d robotPose = GetRobotPose(); + + if(str::IsOnRed()) { + goal = goal + 180_deg; + } + thetaController.SetGoal(goal); + + units::radians_per_second_t thetaSpeed{ + thetaController.Calculate( + robotPose.Rotation().Radians())}; + + swerveDrive.Drive(xVel(), yVel(), thetaSpeed, true); + }, {this})).Until([override] { + return override(); + }); +} + frc2::CommandPtr SwerveSubsystem::FaceSpeaker( std::function xVel, std::function yVel) { diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h index 90063b8..0290d5d 100644 --- a/src/main/include/subsystems/SwerveSubsystem.h +++ b/src/main/include/subsystems/SwerveSubsystem.h @@ -59,6 +59,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { frc2::CommandPtr PIDToPose(std::function goalPose); frc2::CommandPtr AlignToAmp(); SpeakerSide GetSpeakerSideFromPosition(); + frc2::CommandPtr RotateToAngle(std::function xVel, std::function yVel, std::function goalAngle, std::function override); frc2::CommandPtr FaceSpeaker(std::function xVel, std::function yVel); frc2::CommandPtr SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction dir); frc2::CommandPtr SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction dir);