Logging of ATVision poses

4698RaiderRobotics · Nov 10, 2023 · 37bf7d1 · 37bf7d1
1 parent ab743a9
commit 37bf7d1
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 4 deletions.
diff --git a/src/main/cpp/commands/autonomous/BalanceAuto.cpp b/src/main/cpp/commands/autonomous/BalanceAuto.cpp
@@ -14,8 +14,8 @@
 // For more information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 BalanceAuto::BalanceAuto(Drivetrain* drive, ArmSubsystem* arm, GrabberSubsystem* grabber) {
-  frc::Pose2d redAllianceTargetPoints[2] = { drive->redAllianceGridPoints[3], drive->redAllianceGridPoints[5] };
-  frc::Pose2d blueAllianceTargetPoints[2] = { drive->blueAllianceGridPoints[3], drive->blueAllianceGridPoints[5] };
+  // frc::Pose2d redAllianceTargetPoints[2] = { drive->redAllianceGridPoints[3], drive->redAllianceGridPoints[5] };
+  // frc::Pose2d blueAllianceTargetPoints[2] = { drive->blueAllianceGridPoints[3], drive->blueAllianceGridPoints[5] };
   fmt::print("BalanceAuto::BalanceAuto\n");
 
   units::meter_t auto_drive_distance = 2.25_m;

diff --git a/src/main/cpp/commands/autonomous/PlaceOnlyAuto.cpp b/src/main/cpp/commands/autonomous/PlaceOnlyAuto.cpp
@@ -11,8 +11,8 @@
 // For more information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 PlaceOnlyAuto::PlaceOnlyAuto( Drivetrain *drive, ArmSubsystem *arm, GrabberSubsystem *grabber ) {
-  frc::Pose2d redAllianceTargetPoints[2] = { drive->redAllianceGridPoints[3], drive->redAllianceGridPoints[5] };
-  frc::Pose2d blueAllianceTargetPoints[2] = { drive->blueAllianceGridPoints[3], drive->blueAllianceGridPoints[5] };
+  // frc::Pose2d redAllianceTargetPoints[2] = { drive->redAllianceGridPoints[3], drive->redAllianceGridPoints[5] };
+  // frc::Pose2d blueAllianceTargetPoints[2] = { drive->blueAllianceGridPoints[3], drive->blueAllianceGridPoints[5] };
   fmt::print( "PlaceOnlyAuto::PlaceOnlyAuto\n" );
   // If on blue side, do blue auto
   if ( drive->GetPose().X() < 7.5_m ) {

diff --git a/src/main/cpp/subsystems/Limelight.cpp b/src/main/cpp/subsystems/Limelight.cpp
@@ -2,11 +2,14 @@
 #include <iostream>
 
 #include <frc/Timer.h>
+#include <frc/DataLogManager.h>
 
 #include "subsystems/Limelight.h"
 
 Limelight::Limelight()
 {
+    m_poseLogEntry = wpi::log::DoubleArrayLogEntry( frc::DataLogManager::GetLog(), "/ATVision/Robot2D" );
+
 //    frc::SmartDashboard::PutData("Field2", &m_field);
 }
 
@@ -56,6 +59,10 @@ bool Limelight::getFieldAprilTagPose( frc::Pose2d&april_tag_pose, units::second_
     // See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#using-wpilib-s-pose-estimator
     timestamp = frc::Timer::GetFPGATimestamp() - units::second_t{ botpose[6]/1000.0 };
 
+    m_poseLogEntry.Append( { april_tag_pose.X().value(),
+                            april_tag_pose.Y().value(),
+                            april_tag_pose.Rotation().Degrees().value() }, timestamp.value() );
+
     return true;
 }
 

diff --git a/src/main/include/subsystems/Limelight.h b/src/main/include/subsystems/Limelight.h
@@ -12,6 +12,9 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc2/command/SubsystemBase.h>
+
+#include <wpi/DataLog.h>
+
 #include "Constants.h"
 
 class Limelight : public frc2::SubsystemBase {
@@ -46,4 +49,5 @@ class Limelight : public frc2::SubsystemBase {
         frc::Pose2d l_Pose;
         units::degree_t rZ;
         frc::Pose2d t_speeds;
+        wpi::log::DoubleArrayLogEntry m_poseLogEntry;
 };