added align to pass and amp button

frc2053 · Oct 31, 2024 · 6c74844 · 6c74844
1 parent e681b86
commit 6c74844
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 2 deletions.
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -68,6 +68,7 @@ void RobotContainer::ConfigureBindings() {
       RumbleDriver([] { return .5_s; }), RumbleOperator([] { return .5_s; })));
 
   driverController.A().WhileTrue(swerveSubsystem.AlignToAmp());
+  driverController.Y().WhileTrue(swerveSubsystem.AlignToPass());
 
   driverController.Start().OnTrue(swerveSubsystem.ZeroYaw());
 

diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp
@@ -154,6 +154,16 @@ frc2::CommandPtr SwerveSubsystem::FaceSpeaker(
     thetaController.SetTolerance(
         consts::swerve::pathplanning::rotationalPIDTolerance,
         consts::swerve::pathplanning::rotationalVelPIDTolerance);
+
+    frc::Translation2d speakerLocation = consts::yearSpecific::speakerLocationCenter;
+    if (str::IsOnRed()) {
+      speakerLocation = pathplanner::FlippingUtil::flipFieldPosition(speakerLocation);
+    }
+    frc::Pose2d robotPose = GetRobotPose();
+    frc::Translation2d diff = robotPose.Translation() - speakerLocation;
+    units::radian_t angleToSpeaker = units::math::atan2(diff.Y(), diff.X());
+    angleToSpeaker = angleToSpeaker + 180_deg;
+    thetaController.SetGoal(angleToSpeaker);
   }, {this}),
   frc2::cmd::Run([this, xVel, yVel] {
     frc::Translation2d speakerLocation = consts::yearSpecific::speakerLocationCenter;
@@ -623,6 +633,28 @@ frc2::CommandPtr SwerveSubsystem::AlignToAmp() {
       .WithName("AlignToAmp");
 }
 
+frc::Pose2d SwerveSubsystem::GetPassPose() {
+  frc::Pose2d robotPose = GetRobotPose();
+
+  frc::Translation2d passPosition = consts::yearSpecific::passPosition;
+  frc::Translation2d ampLocation = consts::yearSpecific::ampLocation;
+  if(str::IsOnRed()) {
+    passPosition = pathplanner::FlippingUtil::flipFieldPosition(passPosition);
+    ampLocation = pathplanner::FlippingUtil::flipFieldPosition(ampLocation);
+  }
+
+  frc::Translation2d diff = robotPose.Translation() - ampLocation;
+  units::radian_t angleToAmp = units::math::atan2(diff.Y(), diff.X());
+  return frc::Pose2d{passPosition, frc::Rotation2d{angleToAmp}.RotateBy(180_deg)};
+}
+
+frc2::CommandPtr SwerveSubsystem::AlignToPass() {
+  return PIDToPose([this] {
+           return GetPassPose();
+         })
+      .WithName("AlignToPass");
+}
+
 frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4iQuasistaticTorque(
     frc2::sysid::Direction dir) {
   return frc2::cmd::Sequence(

diff --git a/src/main/include/constants/Constants.h b/src/main/include/constants/Constants.h
@@ -16,9 +16,10 @@ inline constexpr units::second_t SWERVE_ODOM_LOOP_PERIOD = 1 / 250_Hz;
 namespace yearSpecific {
 inline const frc::AprilTagFieldLayout aprilTagLayout =
     frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo);
-inline constexpr frc::Translation2d inFrontOfAmpLocation{1.838_m, 7.172_m};
-inline constexpr frc::Translation2d ampLocation{1.838_m, 7.782_m};
+inline constexpr frc::Translation2d inFrontOfAmpLocation{1.838_m, 7.239_m};
+inline constexpr frc::Translation2d ampLocation{1.838_m, 7.882_m};
 inline constexpr units::meter_t closeToAmpDistance{5_ft};
+inline constexpr frc::Translation2d passPosition{8.226_m, 0.615_m};
 inline constexpr frc::Translation2d speakerLocationCenter{0_ft, 5.57_m};
 inline constexpr frc::Translation2d speakerLocationAmpSide{0_ft, 5.9_m};
 inline constexpr frc::Translation2d speakerLocationSourceSide{0_ft, 5.31_m};

diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h
@@ -57,7 +57,9 @@ class SwerveSubsystem : public frc2::SubsystemBase {
                          std::function<units::radians_per_second_t()> omega,
                          bool fieldRelative, bool openLoop = false);
   frc2::CommandPtr PIDToPose(std::function<frc::Pose2d()> goalPose);
+  frc2::CommandPtr AlignToPass();
   frc2::CommandPtr AlignToAmp();
+  frc::Pose2d GetPassPose();
   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);