added rotate to angle command

frc2053 · Oct 22, 2024 · edfb8a2 · edfb8a2
1 parent 5928659
commit edfb8a2
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 0 deletions.
diff --git 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<units::meters_per_second_t()> xVel, 
+  std::function<units::meters_per_second_t()> yVel, 
+  std::function<units::radian_t()> goalAngle, 
+  std::function<bool()> 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<units::meters_per_second_t()> xVel,
   std::function<units::meters_per_second_t()> yVel) {

diff --git 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<frc::Pose2d()> goalPose);
   frc2::CommandPtr AlignToAmp();
   SpeakerSide GetSpeakerSideFromPosition();
+  frc2::CommandPtr RotateToAngle(std::function<units::meters_per_second_t()> xVel, std::function<units::meters_per_second_t()> yVel, std::function<units::radian_t()> goalAngle, std::function<bool()> override);
   frc2::CommandPtr FaceSpeaker(std::function<units::meters_per_second_t()> xVel, std::function<units::meters_per_second_t()> yVel);
   frc2::CommandPtr SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction dir);
   frc2::CommandPtr SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction dir);