3d Pose orientation WORKING PROGRESS + Saum's Cool Sim Stuff! :)

simgui.json

@@ -62,7 +62,8 @@
   "NetworkTables Info": {
     "Clients": {
       "open": true
-    }
+    },
+    "visible": true
   },
   "Plot": {
     "Plot <0>": {
@@ -211,7 +212,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 332
+          "height": 0
         }
       ]
     }

src/main/cpp/commands/VisionCommands.cpp

@@ -0,0 +1,30 @@
+#include "subsystems/SubVision.h"
+#include "subsystems/SubDrivebase.h"
+#include "commands/VisionCommands.h"
+#include <frc/DriverStation.h>
+#include "RobotContainer.h"
+#include <frc/DriverStation.h>
+
+namespace cmd {
+using namespace frc2::cmd;
+frc2::CommandPtr AddVisionMeasurement() {
+  return Run(
+      [] {
+        auto estimatedPose = SubVision::GetInstance().GetPose();
+        frc::SmartDashboard::PutBoolean("Vision/is teleop", frc::DriverStation::IsTeleop());
+        frc::SmartDashboard::PutBoolean("Vision/has value", estimatedPose.has_value());
+        if (estimatedPose.has_value() && frc::DriverStation::IsTeleop()) {
+          auto estimatedPoseValue = estimatedPose.value();
+          SubDrivebase::GetInstance().AddVisionMeasurement(
+              estimatedPoseValue.estimatedPose.ToPose2d(), 0,
+              estimatedPoseValue.timestamp);
+          SubDrivebase::GetInstance().DisplayPose(
+              "EstimatedPose", estimatedPoseValue.estimatedPose.ToPose2d());
+        } else {
+          SubDrivebase::GetInstance().DisplayPose("EstimatedPose", {});
+        }
+      },
+      {&SubVision::GetInstance()});
+}
+}
+

src/main/cpp/subsystems/SubDrivebase.cpp

@@ -136,6 +136,18 @@ void SubDrivebase::SimulationPeriodic() {
   _frontRight.UpdateSim(20_ms);
   _backLeft.UpdateSim(20_ms);
   _backRight.UpdateSim(20_ms);
+
+  auto fl = _frontLeft.GetPosition();
+  auto fr = _frontRight.GetPosition();
+  auto bl = _backLeft.GetPosition();
+  auto br = _backRight.GetPosition();
+
+  _simPoseEstimator.Update(GetHeading(), {fl, fr, bl, br});
+  DisplayPose("simRobotPose", _simPoseEstimator.GetEstimatedPosition());
+}
+
+frc::Pose2d SubDrivebase::GetSimPose(){
+  return _simPoseEstimator.GetEstimatedPosition();
 }
 
 frc2::CommandPtr SubDrivebase::JoystickDrive(frc2::CommandXboxController &controller,

src/main/cpp/subsystems/SubVision.cpp

@@ -5,6 +5,7 @@
 #include "subsystems/SubVision.h"
 #include "subsystems/SubDrivebase.h"
 #include <frc/DriverStation.h>
+#include "commands/VisionCommands.h"
 
 SubVision::SubVision() {
   for (int i = 0; i <= 18; i++) {
@@ -15,6 +16,12 @@ SubVision::SubVision() {
       SubDrivebase::GetInstance().DisplayPose(fmt::format("tag{}", i), pose.value().ToPose2d());
     }
   }
+
+  SetDefaultCommand(cmd::AddVisionMeasurement());
+}
+
+std::optional<photon::EstimatedRobotPose> SubVision::GetPose() {
+  return _visionPoseEstimator.Update();
 }
 
 // This method will be called once per scheduler run
@@ -26,7 +33,7 @@ void SubVision::Periodic() {
   }
 
 void SubVision::SimulationPeriodic() {
-  _visionSim.ProcessFrame(SubDrivebase::GetInstance().GetPose());
+  _visionSim.ProcessFrame(SubDrivebase::GetInstance().GetSimPose());
 };
 
 std::optional<photon::PhotonTrackedTarget> SubVision::GetSpeakerTarget() {
@@ -84,4 +91,5 @@ bool SubVision::IsFacingTarget(){
   } else {
     return false;
   }
-}
+
+}

src/main/include/commands/VisionCommands.h

@@ -0,0 +1,6 @@
+#include <frc/geometry/Pose3d.h>
+#include <frc2/command/Commands.h>
+
+namespace cmd{
+frc2::CommandPtr AddVisionMeasurement();
+}

src/main/include/subsystems/SubDrivebase.h

@@ -143,4 +143,18 @@ class SubDrivebase : public frc2::SubsystemBase {
                                    .velocity(_frontRight.GetSpeed());
                              },
                              this}};
-};
+
+  //Saum's Cool Simulation stuff
+frc::SwerveDrivePoseEstimator<4> _simPoseEstimator{
+      _kinematics,
+      _gyro.GetRotation2d(),
+      {frc::SwerveModulePosition{0_m, _frontLeft.GetAngle()},
+       frc::SwerveModulePosition{0_m, _frontRight.GetAngle()},
+       frc::SwerveModulePosition{0_m, _backLeft.GetAngle()},
+       frc::SwerveModulePosition{0_m, _backRight.GetAngle()}},
+      frc::Pose2d()};
+
+public:
+  frc::Pose2d GetSimPose();
+
+};

src/main/include/subsystems/SubVision.h

@@ -37,23 +37,27 @@ class SubVision : public frc2::SubsystemBase {
   std::optional<photon::PhotonTrackedTarget> GetSpeakerTarget();
   std::optional<units::degree_t> GetSpeakerYaw();
   std::optional<units::degree_t> GetSpeakerPitch();
+  std::optional<photon::EstimatedRobotPose> GetPose();
 
   bool IsFacingTarget();
 
 private:
   std::string CAM_NAME1 = "arducam";
 
-  frc::Transform3d _camToBot{{0_mm, -200_mm, -150_mm}, {}}; // arducam
+  frc::Transform3d _camToBot{{0_mm, -200_mm, -150_mm}, {}}; // arducam FIX
 
   frc::AprilTagFieldLayout _tagLayout = frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo);
 
-  // photonlib::PhotonPoseEstimator _visionPoseEstimator{
-  //     _tagLayout,
-  //     photonlib::PoseStrategy::MULTI_TAG_PNP,
-  //     photonlib::PhotonCamera{CAM_NAME1},
-  //     _camToBot.Inverse()};
+  photon::PhotonPoseEstimator _visionPoseEstimator{
+      _tagLayout,
+      photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
+      photon::PhotonCamera{CAM_NAME1},
+      _camToBot.Inverse()};
 
 
+
+
+
 
   photon::PhotonCamera camera{CAM_NAME1};