improved pid to pose

frc2053 · Sep 13, 2024 · c83f2be · c83f2be
1 parent f48bdd7
commit c83f2be
Show file tree

Hide file tree

Showing 5 changed files with 53 additions and 50 deletions.
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -29,11 +29,11 @@ void RobotContainer::ConfigureBindings() {
       true, true));
 
   controller.LeftTrigger().WhileTrue(swerveSubsystem.AlignToAmp());
-  // controller.LeftBumper().WhileTrue(
-  //   swerveSubsystem.PIDToPose([this] {
-
-  //   })
-  // );
+  controller.LeftBumper().WhileTrue(
+    swerveSubsystem.PIDToPose([this] {
+      return swerveSubsystem.GetFoundNotePose();
+    })
+  );
   // controller.Back().WhileTrue(
   //     swerveSubsystem.WheelRadius(frc2::sysid::Direction::kReverse));
   // controller.Start().WhileTrue(

diff --git a/src/main/cpp/str/Camera.cpp b/src/main/cpp/str/Camera.cpp
@@ -75,39 +75,33 @@ std::optional<units::meter_t> Camera::GetDistanceToNote() {
   double max_width = 0;
   double best_yaw = 0;
   double pixelToRad = 20;
-  bool didReadData = false;
-  for (const auto &result : camera->GetAllUnreadResults()) {
-    didReadData = true;
-    if (result.HasTargets()) {
-      for (const auto &target : result.GetTargets()) {
-        if (target.GetFiducialId() == -1) {
-          double width = 0;
-          double yaw = 0;
-          std::vector<photon::TargetCorner> corners =
-              target.GetDetectedCorners();
-          double minX = corners[0].x;
-          double maxX = corners[0].x;
-
-          for (const auto &corner : corners) {
-            minX = std::min(minX, corner.x);
-            maxX = std::max(maxX, corner.x);
-          }
-
-          width = maxX - minX;
-          yaw = target.GetYaw();
-
-          if(target.GetArea() > max_area) {
-            max_area = target.GetArea();
-            max_width = width;
-            best_yaw = yaw;
-          }
+  photon::PhotonPipelineResult result = camera->GetLatestResult();
+  if (result.HasTargets()) {
+    for (const auto &target : result.GetTargets()) {
+      if (target.GetFiducialId() == -1) {
+        double width = 0;
+        double yaw = 0;
+        std::vector<photon::TargetCorner> corners =
+            target.GetDetectedCorners();
+        double minX = corners[0].x;
+        double maxX = corners[0].x;
+
+        for (const auto &corner : corners) {
+          minX = std::min(minX, corner.x);
+          maxX = std::max(maxX, corner.x);
+        }
+
+        width = maxX - minX;
+        yaw = target.GetYaw();
+
+        if(target.GetArea() > max_area) {
+          max_area = target.GetArea();
+          max_width = width;
+          best_yaw = yaw;
         }
       }
     }
   }
-  if(!didReadData) {
-    fmt::print("data not found!\n");
-  }
   if (max_width != 0) {
     angleToNote = std::make_optional(units::degree_t{best_yaw});
     fmt::print("max width: {}\n", max_width);

diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp
@@ -177,21 +177,24 @@ bool SwerveSubsystem::IsNearAmp() {
          consts::yearSpecific::closeToAmpDistance;
 }
 
-frc::Pose2d SwerveSubsystem::CalculateFoundNotePose(std::optional<units::meter_t> distanceToNote, std::optional<units::radian_t> angleToNote) {
+void SwerveSubsystem::CalculateFoundNotePose(std::optional<units::meter_t> distanceToNote, std::optional<units::radian_t> angleToNote) {
   frc::Pose3d robotPose = frc::Pose3d{GetRobotPose()};
-  units::meter_t dist = cachedNoteDist;
-  auto noteDistOpt = distanceToNote;
-  if(noteDistOpt.has_value()) {
-    dist = noteDistOpt.value();
-    cachedNoteDist = dist;
+  if(!angleToNote.has_value() || !distanceToNote.has_value()) {
+    latestNotePose = robotPose.ToPose2d();
   }
-  units::radian_t yaw = angleToNote.value_or(0_deg);
-  noteDistPub.Set(dist.value());
-  noteYawPub.Set(yaw.convert<units::degrees>().value());
-  frc::Transform3d camToNote{dist * units::math::cos(-yaw), dist * units::math::sin(-yaw), 0_m, frc::Rotation3d{0_rad, 0_rad, 0_rad}};
-  frc::Pose3d notePose = robotPose.TransformBy(consts::vision::ROBOT_TO_NOTE_CAM).TransformBy(camToNote);
-  foundNotePose.Set(frc::Pose3d{notePose.X(), notePose.Y(), 0_m, frc::Rotation3d{}});
-  return frc::Pose2d{notePose.X(), notePose.Y(), frc::Rotation2d{}};
+  else {
+    units::radian_t yaw = angleToNote.value_or(0_deg);
+    noteDistPub.Set(distanceToNote.value().value());
+    noteYawPub.Set(yaw.convert<units::degrees>().value());
+    frc::Transform3d camToNote{distanceToNote.value() * units::math::cos(-yaw), distanceToNote.value() * units::math::sin(-yaw), 0_m, frc::Rotation3d{0_rad, 0_rad, 0_rad}};
+    frc::Pose3d notePose = robotPose.TransformBy(consts::vision::ROBOT_TO_NOTE_CAM).TransformBy(camToNote);
+    foundNotePose.Set(frc::Pose3d{notePose.X(), notePose.Y(), 0_m, frc::Rotation3d{}});
+    latestNotePose = frc::Pose2d{notePose.X(), notePose.Y(), frc::Rotation2d{}};
+  }
+}
+
+frc::Pose2d SwerveSubsystem::GetFoundNotePose() const {
+  return latestNotePose;
 }
 
 frc2::CommandPtr
@@ -246,6 +249,11 @@ SwerveSubsystem::PIDToPose(std::function<frc::Pose2d()> goalPose) {
                  [this, goalPose] {
                    frc::Pose2d currentPose = GetRobotPose();
 
+                   xPoseController.SetGoal(goalPose().X());
+                   yPoseController.SetGoal(goalPose().Y());
+                   thetaController.SetGoal(goalPose().Rotation().Radians());
+                   pidPoseSetpointPub.Set(goalPose());
+
                    units::meters_per_second_t xSpeed{xPoseController.Calculate(
                        currentPose.Translation().X())};
                    units::meters_per_second_t ySpeed{yPoseController.Calculate(

diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-4fc4e50
+f48bdd7
diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h
@@ -62,8 +62,8 @@ class SwerveSubsystem : public frc2::SubsystemBase {
   frc2::CommandPtr WheelRadius(frc2::sysid::Direction dir);
   frc2::CommandPtr TuneSteerPID(std::function<bool()> isDone);
   frc2::CommandPtr TuneDrivePID(std::function<bool()> isDone);
-  frc::Pose2d CalculateFoundNotePose(std::optional<units::meter_t> distanceToNote, std::optional<units::radian_t> angleToNote);
-
+  void CalculateFoundNotePose(std::optional<units::meter_t> distanceToNote, std::optional<units::radian_t> angleToNote);
+  frc::Pose2d GetFoundNotePose() const;
 private:
   void SetupPathplanner();
   void LoadChoreoTrajectories();
@@ -72,6 +72,7 @@ class SwerveSubsystem : public frc2::SubsystemBase {
   bool IsNearAmp();
 
   units::meter_t cachedNoteDist;
+  frc::Pose2d latestNotePose;
 
   nt::StructPublisher<frc::Pose3d> foundNotePose{
     nt::NetworkTableInstance::GetDefault()