From d20666f1fae4d0901e598d4ca97de9e77babcc25 Mon Sep 17 00:00:00 2001 From: Samuel Wiseman <> Date: Fri, 19 Jan 2024 15:37:40 -0600 Subject: [PATCH 1/5] lifeofatruebrexitgeezer --- src/RobotContainer.cpp | 8 ++-- src/RobotContainer.h | 5 +- src/subsystems/vision/VisionSubsystem.cpp | 44 +++++++++++++++++ src/subsystems/vision/VisionSubsystem.h | 44 +++++++++++++++++ vendordeps/PathplannerLib.json | 8 ++-- vendordeps/photonlib.json | 57 +++++++++++++++++++++++ 6 files changed, 156 insertions(+), 10 deletions(-) create mode 100644 src/subsystems/vision/VisionSubsystem.cpp create mode 100644 src/subsystems/vision/VisionSubsystem.h create mode 100644 vendordeps/photonlib.json diff --git a/src/RobotContainer.cpp b/src/RobotContainer.cpp index ab80270..98fd236 100644 --- a/src/RobotContainer.cpp +++ b/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()); } diff --git a/src/RobotContainer.h b/src/RobotContainer.h index 66419b6..ef654c3 100644 --- a/src/RobotContainer.h +++ b/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 @@ -36,8 +37,8 @@ class RobotContainer { // The robot's subsystems are defined here... std::shared_ptr gyro = std::make_shared(constants::gyroPort); - DriveSubsystem driveSubsystem; - + //DriveSubsystem driveSubsystem; + VisionSubsystem visionSubsystem; rmb::LogitechGamepad gamepad{constants::driverControllerPort}; void ConfigureBindings(); diff --git a/src/subsystems/vision/VisionSubsystem.cpp b/src/subsystems/vision/VisionSubsystem.cpp new file mode 100644 index 0000000..0c226f9 --- /dev/null +++ b/src/subsystems/vision/VisionSubsystem.cpp @@ -0,0 +1,44 @@ +// 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() // : m_PhotonCamera("TESTNAME") +{ + + const std::string_view cameraName = "TESTNAME"; + this->m_PhotonCamera = new photon::PhotonCamera{cameraName}; + std::cout << "Vision Subsystem constructor" << std::endl; +} + +frc2::CommandPtr VisionSubsystem::ExampleMethodCommand() +{ + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return RunOnce([/* this */] { /* one-time action goes here */ }); +} + +bool VisionSubsystem::ExampleCondition() +{ + // Query some boolean state, such as a digital sensor. + return false; +} + +void VisionSubsystem::Periodic() +{ + photon::PhotonPipelineResult result = this->m_PhotonCamera->GetLatestResult(); + if (!result.HasTargets()) + { + //std::cout << "No AprilTag(s) found" << std::endl; + return; + } + + photon::PhotonTrackedTarget bestTarget = result.GetBestTarget(); + std::cout << "ID: " << bestTarget.GetFiducialId() << std::endl; +} + +void VisionSubsystem::SimulationPeriodic() +{ + // Implementation of subsystem simulation periodic method goes here. +} diff --git a/src/subsystems/vision/VisionSubsystem.h b/src/subsystems/vision/VisionSubsystem.h new file mode 100644 index 0000000..1e659b9 --- /dev/null +++ b/src/subsystems/vision/VisionSubsystem.h @@ -0,0 +1,44 @@ +// 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 +#include + +#include +#include +#include + +class VisionSubsystem : public frc2::SubsystemBase { +public: + VisionSubsystem(); + + /** + * Example command factory method. + */ + frc2::CommandPtr ExampleMethodCommand(); + + /** + * An example method querying a boolean state of the subsystem (for example, a + * digital sensor). + * + * @return value of some boolean subsystem state, such as a digital sensor. + */ + bool ExampleCondition(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + /** + * Will be called periodically whenever the CommandScheduler runs during + * simulation. + */ + void SimulationPeriodic() override; + +private: + photon::PhotonCamera *m_PhotonCamera; +}; diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 62caa4c..f86862c 100644 --- a/vendordeps/PathplannerLib.json +++ b/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,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.2" + "version": "2024.1.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "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 @@ ] } ] -} +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..282cfa8 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.0", + "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.0", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.0", + "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.0" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.0" + } + ] +} \ No newline at end of file From 5d8250f672af15421c7e0657b750934e6c49a1db Mon Sep 17 00:00:00 2001 From: notSam25 Date: Fri, 19 Jan 2024 21:42:43 +0000 Subject: [PATCH 2/5] BOT: Apply Formatting --- src/RobotContainer.cpp | 4 ++-- src/RobotContainer.h | 2 +- src/subsystems/vision/VisionSubsystem.cpp | 17 ++++++----------- src/subsystems/vision/VisionSubsystem.h | 2 +- 4 files changed, 10 insertions(+), 15 deletions(-) diff --git a/src/RobotContainer.cpp b/src/RobotContainer.cpp index 98fd236..fce6fac 100644 --- a/src/RobotContainer.cpp +++ b/src/RobotContainer.cpp @@ -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( + // driveSubsystem.SetDefaultCommand( // frc2::RunCommand([this] { driveSubsystem.stop(); }).ToPtr()); } diff --git a/src/RobotContainer.h b/src/RobotContainer.h index ef654c3..6b6fde8 100644 --- a/src/RobotContainer.h +++ b/src/RobotContainer.h @@ -37,7 +37,7 @@ class RobotContainer { // The robot's subsystems are defined here... std::shared_ptr gyro = std::make_shared(constants::gyroPort); - //DriveSubsystem driveSubsystem; + // DriveSubsystem driveSubsystem; VisionSubsystem visionSubsystem; rmb::LogitechGamepad gamepad{constants::driverControllerPort}; diff --git a/src/subsystems/vision/VisionSubsystem.cpp b/src/subsystems/vision/VisionSubsystem.cpp index 0c226f9..5086392 100644 --- a/src/subsystems/vision/VisionSubsystem.cpp +++ b/src/subsystems/vision/VisionSubsystem.cpp @@ -12,25 +12,21 @@ VisionSubsystem::VisionSubsystem() // : m_PhotonCamera("TESTNAME") std::cout << "Vision Subsystem constructor" << std::endl; } -frc2::CommandPtr VisionSubsystem::ExampleMethodCommand() -{ +frc2::CommandPtr VisionSubsystem::ExampleMethodCommand() { // Inline construction of command goes here. // Subsystem::RunOnce implicitly requires `this` subsystem. return RunOnce([/* this */] { /* one-time action goes here */ }); } -bool VisionSubsystem::ExampleCondition() -{ +bool VisionSubsystem::ExampleCondition() { // Query some boolean state, such as a digital sensor. return false; } -void VisionSubsystem::Periodic() -{ +void VisionSubsystem::Periodic() { photon::PhotonPipelineResult result = this->m_PhotonCamera->GetLatestResult(); - if (!result.HasTargets()) - { - //std::cout << "No AprilTag(s) found" << std::endl; + if (!result.HasTargets()) { + // std::cout << "No AprilTag(s) found" << std::endl; return; } @@ -38,7 +34,6 @@ void VisionSubsystem::Periodic() std::cout << "ID: " << bestTarget.GetFiducialId() << std::endl; } -void VisionSubsystem::SimulationPeriodic() -{ +void VisionSubsystem::SimulationPeriodic() { // Implementation of subsystem simulation periodic method goes here. } diff --git a/src/subsystems/vision/VisionSubsystem.h b/src/subsystems/vision/VisionSubsystem.h index 1e659b9..1e0114a 100644 --- a/src/subsystems/vision/VisionSubsystem.h +++ b/src/subsystems/vision/VisionSubsystem.h @@ -8,8 +8,8 @@ #include #include -#include #include +#include class VisionSubsystem : public frc2::SubsystemBase { public: From 4685afa5f4e88ebf684bc8f6a8206caba9bbc32d Mon Sep 17 00:00:00 2001 From: Aiden Lambert <74067227+theVerySharpFlat@users.noreply.github.com> Date: Mon, 22 Jan 2024 21:48:47 -0600 Subject: [PATCH 3/5] Bump photonlib --- vendordeps/photonlib.json | 112 +++++++++++++++++++------------------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 282cfa8..294c339 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,57 +1,57 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.2.0", - "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.0", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.2.0", - "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.0" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.2.0" - } - ] -} \ No newline at end of file + "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" + } + ] +} From 99d1e0d1c59538a21702f056aac3dd770323cb53 Mon Sep 17 00:00:00 2001 From: Samuel Wiseman <> Date: Mon, 5 Feb 2024 13:15:16 -0600 Subject: [PATCH 4/5] just some crappy code that needs to be put into main --- src/RobotContainer.h | 4 +- src/subsystems/vision/VisionSubsystem.cpp | 58 ++++++++++++++--------- src/subsystems/vision/VisionSubsystem.h | 41 +++++----------- 3 files changed, 51 insertions(+), 52 deletions(-) diff --git a/src/RobotContainer.h b/src/RobotContainer.h index 6b6fde8..02090d1 100644 --- a/src/RobotContainer.h +++ b/src/RobotContainer.h @@ -35,8 +35,8 @@ class RobotContainer { // constants::driverControllerPort}; // The robot's subsystems are defined here... - std::shared_ptr gyro = - std::make_shared(constants::gyroPort); + //std::shared_ptr gyro = + //std::make_shared(constants::gyroPort); // DriveSubsystem driveSubsystem; VisionSubsystem visionSubsystem; rmb::LogitechGamepad gamepad{constants::driverControllerPort}; diff --git a/src/subsystems/vision/VisionSubsystem.cpp b/src/subsystems/vision/VisionSubsystem.cpp index 5086392..8a2602b 100644 --- a/src/subsystems/vision/VisionSubsystem.cpp +++ b/src/subsystems/vision/VisionSubsystem.cpp @@ -4,36 +4,50 @@ #include "subsystems/vision/VisionSubsystem.h" -VisionSubsystem::VisionSubsystem() // : m_PhotonCamera("TESTNAME") +VisionSubsystem::VisionSubsystem() { - const std::string_view cameraName = "TESTNAME"; - this->m_PhotonCamera = new photon::PhotonCamera{cameraName}; - std::cout << "Vision Subsystem constructor" << std::endl; -} + // 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); -frc2::CommandPtr VisionSubsystem::ExampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return RunOnce([/* this */] { /* one-time action goes here */ }); } -bool VisionSubsystem::ExampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; +VisionSubsystem::~VisionSubsystem() { + delete this->m_PoseEstimator; } -void VisionSubsystem::Periodic() { - photon::PhotonPipelineResult result = this->m_PhotonCamera->GetLatestResult(); - if (!result.HasTargets()) { - // std::cout << "No AprilTag(s) found" << std::endl; - return; +std::pair VisionSubsystem::getEstimatedGlobalPose( + frc::Pose3d prevEstimatedRobotPose) +{ + this->m_PoseEstimator->SetReferencePose(prevEstimatedRobotPose); + units::millisecond_t currentTime = frc::Timer::GetFPGATimestamp(); + std::optional 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); } - - photon::PhotonTrackedTarget bestTarget = result.GetBestTarget(); - std::cout << "ID: " << bestTarget.GetFiducialId() << std::endl; } -void VisionSubsystem::SimulationPeriodic() { - // Implementation of subsystem simulation periodic method goes here. +void VisionSubsystem::Periodic() +{ + // I know these are errors, they are placeholders. + std::pair 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) + } } diff --git a/src/subsystems/vision/VisionSubsystem.h b/src/subsystems/vision/VisionSubsystem.h index 1e0114a..91a669e 100644 --- a/src/subsystems/vision/VisionSubsystem.h +++ b/src/subsystems/vision/VisionSubsystem.h @@ -6,39 +6,24 @@ #include #include - -#include -#include +#include +#include +#include +#include #include - -class VisionSubsystem : public frc2::SubsystemBase { +#include +#include +#include +#include "../drive/DriveSubsystem.h" +class VisionSubsystem : public frc2::SubsystemBase +{ public: VisionSubsystem(); + ~VisionSubsystem(); - /** - * Example command factory method. - */ - frc2::CommandPtr ExampleMethodCommand(); - - /** - * An example method querying a boolean state of the subsystem (for example, a - * digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - bool ExampleCondition(); - - /** - * Will be called periodically whenever the CommandScheduler runs. - */ void Periodic() override; - /** - * Will be called periodically whenever the CommandScheduler runs during - * simulation. - */ - void SimulationPeriodic() override; - private: - photon::PhotonCamera *m_PhotonCamera; + std::pair getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose); + photon::PhotonPoseEstimator *m_PoseEstimator; }; From ef035155168f3d154ee5567613140b044a232d57 Mon Sep 17 00:00:00 2001 From: notSam25 Date: Mon, 5 Feb 2024 19:19:51 +0000 Subject: [PATCH 5/5] BOT: Apply Formatting --- src/RobotContainer.h | 4 +- src/subsystems/vision/VisionSubsystem.cpp | 52 ++++++++++------------- src/subsystems/vision/VisionSubsystem.h | 20 ++++----- 3 files changed, 34 insertions(+), 42 deletions(-) diff --git a/src/RobotContainer.h b/src/RobotContainer.h index 02090d1..7dfc559 100644 --- a/src/RobotContainer.h +++ b/src/RobotContainer.h @@ -35,8 +35,8 @@ class RobotContainer { // constants::driverControllerPort}; // The robot's subsystems are defined here... - //std::shared_ptr gyro = - //std::make_shared(constants::gyroPort); + // std::shared_ptr gyro = + // std::make_shared(constants::gyroPort); // DriveSubsystem driveSubsystem; VisionSubsystem visionSubsystem; rmb::LogitechGamepad gamepad{constants::driverControllerPort}; diff --git a/src/subsystems/vision/VisionSubsystem.cpp b/src/subsystems/vision/VisionSubsystem.cpp index 8a2602b..0daa5aa 100644 --- a/src/subsystems/vision/VisionSubsystem.cpp +++ b/src/subsystems/vision/VisionSubsystem.cpp @@ -4,50 +4,42 @@ #include "subsystems/vision/VisionSubsystem.h" -VisionSubsystem::VisionSubsystem() -{ +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); - + 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; -} +VisionSubsystem::~VisionSubsystem() { delete this->m_PoseEstimator; } -std::pair VisionSubsystem::getEstimatedGlobalPose( - frc::Pose3d prevEstimatedRobotPose) -{ +std::pair +VisionSubsystem::getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose) { this->m_PoseEstimator->SetReferencePose(prevEstimatedRobotPose); units::millisecond_t currentTime = frc::Timer::GetFPGATimestamp(); - std::optional result = this->m_PoseEstimator->Update(); - if (result.has_value()) - { + std::optional result = + this->m_PoseEstimator->Update(); + if (result.has_value()) { return std::make_pair<>(result->estimatedPose, currentTime - result->timestamp); - } - else - { + } else { return std::make_pair(frc::Pose3d(), 0_ms); } } -void VisionSubsystem::Periodic() -{ +void VisionSubsystem::Periodic() { // I know these are errors, they are placeholders. - std::pair 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()); + std::pair 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) } } diff --git a/src/subsystems/vision/VisionSubsystem.h b/src/subsystems/vision/VisionSubsystem.h index 91a669e..ea04e67 100644 --- a/src/subsystems/vision/VisionSubsystem.h +++ b/src/subsystems/vision/VisionSubsystem.h @@ -4,19 +4,18 @@ #pragma once -#include -#include -#include +#include "../drive/DriveSubsystem.h" #include -#include #include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include "../drive/DriveSubsystem.h" -class VisionSubsystem : public frc2::SubsystemBase -{ +class VisionSubsystem : public frc2::SubsystemBase { public: VisionSubsystem(); ~VisionSubsystem(); @@ -24,6 +23,7 @@ class VisionSubsystem : public frc2::SubsystemBase void Periodic() override; private: - std::pair getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose); + std::pair + getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose); photon::PhotonPoseEstimator *m_PoseEstimator; };