diff --git a/src/RobotContainer.cpp b/src/RobotContainer.cpp index ab80270..fce6fac 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..7dfc559 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 @@ -34,10 +35,10 @@ class RobotContainer { // constants::driverControllerPort}; // The robot's subsystems are defined here... - std::shared_ptr gyro = - std::make_shared(constants::gyroPort); - DriveSubsystem driveSubsystem; - + // std::shared_ptr gyro = + // std::make_shared(constants::gyroPort); + // 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..0daa5aa --- /dev/null +++ b/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 +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); + } +} + +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 new file mode 100644 index 0000000..ea04e67 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +class VisionSubsystem : public frc2::SubsystemBase { +public: + VisionSubsystem(); + ~VisionSubsystem(); + + void Periodic() override; + +private: + std::pair + getEstimatedGlobalPose(frc::Pose3d prevEstimatedRobotPose); + photon::PhotonPoseEstimator *m_PoseEstimator; +}; 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..294c339 --- /dev/null +++ b/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" + } + ] +}