From c83f2becdb389be360587955266da47d8febcc2d Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Fri, 13 Sep 2024 11:34:12 -0400 Subject: [PATCH] improved pid to pose --- src/main/cpp/RobotContainer.cpp | 10 ++-- src/main/cpp/str/Camera.cpp | 52 ++++++++----------- src/main/cpp/subsystems/SwerveSubsystem.cpp | 34 +++++++----- src/main/deploy/commit.txt | 2 +- src/main/include/subsystems/SwerveSubsystem.h | 5 +- 5 files changed, 53 insertions(+), 50 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index de6d412..7a5769a 100644 --- 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 index a1ec1e2..7cafee0 100644 --- a/src/main/cpp/str/Camera.cpp +++ b/src/main/cpp/str/Camera.cpp @@ -75,39 +75,33 @@ std::optional 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 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 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 index 0b55c89..fa1c326 100644 --- 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 distanceToNote, std::optional angleToNote) { +void SwerveSubsystem::CalculateFoundNotePose(std::optional distanceToNote, std::optional 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().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().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 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 index cc4c62c..a04eab0 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -4fc4e50 \ No newline at end of file +f48bdd7 \ No newline at end of file diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h index 24f974f..5e4f55f 100644 --- 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 isDone); frc2::CommandPtr TuneDrivePID(std::function isDone); - frc::Pose2d CalculateFoundNotePose(std::optional distanceToNote, std::optional angleToNote); - + void CalculateFoundNotePose(std::optional distanceToNote, std::optional 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 foundNotePose{ nt::NetworkTableInstance::GetDefault()