Feature/vision

src/RobotContainer.cpp

@@ -10,7 +10,7 @@
 #include "frc2/command/RunCommand.h"
 #include "subsystems/drive/DriveSubsystem.h"
 
-RobotContainer::RobotContainer() : driveSubsystem(gyro) {
+RobotContainer::RobotContainer() /*: driveSubsystem(gyro)*/ {
   // Initialize all of your commands and subsystems here
 
   // Configure the button bindings
@@ -37,10 +37,10 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
 }
 
 void RobotContainer::setTeleopDefaults() {
-  driveSubsystem.SetDefaultCommand(driveSubsystem.driveTeleopCommand(gamepad));
+  // driveSubsystem.SetDefaultCommand(driveSubsystem.driveTeleopCommand(gamepad));
 }
 
 void RobotContainer::setAutoDefaults() {
-  driveSubsystem.SetDefaultCommand(
-      frc2::RunCommand([this] { driveSubsystem.stop(); }).ToPtr());
+  // driveSubsystem.SetDefaultCommand(
+  //    frc2::RunCommand([this] { driveSubsystem.stop(); }).ToPtr());
 }

src/RobotContainer.h

@@ -11,6 +11,7 @@
 #include "rmb/controller/LogitechGamepad.h"
 #include "rmb/sensors/AHRS/AHRSGyro.h"
 #include "subsystems/drive/DriveSubsystem.h"
+#include "subsystems/vision/VisionSubsystem.h"
 
 /**
  * This class is where the bulk of the robot should be declared.  Since
@@ -34,10 +35,10 @@ class RobotContainer {
   //     constants::driverControllerPort};
 
   // The robot's subsystems are defined here...
-  std::shared_ptr<rmb::AHRSGyro> gyro =
-      std::make_shared<rmb::AHRSGyro>(constants::gyroPort);
-  DriveSubsystem driveSubsystem;
-
+  // std::shared_ptr<rmb::AHRSGyro> gyro =
+  // std::make_shared<rmb::AHRSGyro>(constants::gyroPort);
+  // DriveSubsystem driveSubsystem;
+  VisionSubsystem visionSubsystem;
   rmb::LogitechGamepad gamepad{constants::driverControllerPort};
 
   void ConfigureBindings();

src/subsystems/vision/VisionSubsystem.cpp

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/vision/VisionSubsystem.h"
+
+VisionSubsystem::VisionSubsystem() {
+
+  // TODO: Apply camera transormations relative to robot centre.
+  frc::Transform3d robotToCam = frc::Transform3d(
+      frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad));
+
+  frc::AprilTagFieldLayout aprilTags =
+      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo);
+  this->m_PoseEstimator = new photon::PhotonPoseEstimator(
+      aprilTags, photon::MULTI_TAG_PNP_ON_COPROCESSOR,
+      photon::PhotonCamera("TESTCAMERA"), robotToCam);
+}
+
+VisionSubsystem::~VisionSubsystem() { delete this->m_PoseEstimator; }
+
+std::pair<frc::Pose3d, units::millisecond_t>
+VisionSubsystem::getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose) {
+  this->m_PoseEstimator->SetReferencePose(prevEstimatedRobotPose);
+  units::millisecond_t currentTime = frc::Timer::GetFPGATimestamp();
+  std::optional<photon::EstimatedRobotPose> result =
+      this->m_PoseEstimator->Update();
+  if (result.has_value()) {
+    return std::make_pair<>(result->estimatedPose,
+                            currentTime - result->timestamp);
+  } else {
+    return std::make_pair(frc::Pose3d(), 0_ms);
+  }
+}
+
+void VisionSubsystem::Periodic() {
+  // I know these are errors, they are placeholders.
+  std::pair<frc::Pose3d, units::millisecond_t> result =
+      getEstimatedGlobalPose(drivesubsystem->getestimatedglobalpose);
+  if (result.second != 0_ms) {
+    printf("%.3f, %.3f, %.3f", result.first.X().value(),
+           result.first.Y().value(), result.first.Z().value());
+    drivesubsystem->appendglobalpose(result.first)
+  }
+}

src/subsystems/vision/VisionSubsystem.h

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "../drive/DriveSubsystem.h"
+#include <frc/ComputerVisionUtil.h>
+#include <frc/Timer.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/SubsystemBase.h>
+#include <iostream>
+#include <memory>
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonPoseEstimator.h>
+class VisionSubsystem : public frc2::SubsystemBase {
+public:
+  VisionSubsystem();
+  ~VisionSubsystem();
+
+  void Periodic() override;
+
+private:
+  std::pair<frc::Pose3d, units::millisecond_t>
+  getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose);
+  photon::PhotonPoseEstimator *m_PoseEstimator;
+};

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.2",
+    "version": "2024.1.3",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.2"
+            "version": "2024.1.3"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.2",
+            "version": "2024.1.3",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -35,4 +35,4 @@
             ]
         }
     ]
-}
+}

vendordeps/photonlib.json

@@ -0,0 +1,57 @@
+{
+  "fileName": "photonlib.json",
+  "name": "photonlib",
+  "version": "v2024.2.1",
+  "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+  "frcYear": "2024",
+  "mavenUrls": [
+    "https://maven.photonvision.org/repository/internal",
+    "https://maven.photonvision.org/repository/snapshots"
+  ],
+  "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-cpp",
+      "version": "v2024.2.1",
+      "libName": "photonlib",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "v2024.2.1",
+      "libName": "photontargeting",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-java",
+      "version": "v2024.2.1"
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-java",
+      "version": "v2024.2.1"
+    }
+  ]
+}