From 87c92edf2c14161ae30419b6c18d38b327fdfa7d Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 15:08:22 +0800
Subject: [PATCH] Did more vision stuff

---
 src/main/cpp/Robot.cpp           | 11 +++--------
 src/main/cpp/vision/Vision.cpp   |  2 +-
 src/main/include/vision/Vision.h |  2 +-
 3 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 7ba7c016..9fbfaea2 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -138,17 +138,12 @@ void Robot::TeleopInit() {
 
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
-}
 
-void Robot::TeleopPeriodic() {
-  // std::cout << "Current AprilTag: " << _vision->CurrentAprilTag() << std::endl;
-  // std::cout << "Current Target: " << _vision->TargetIsVisible(VisionTargetObjects::kNote) << std::endl;
-  std::cout << "Dist to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).first.value()
-            << std::endl;
-  std::cout << "Angle to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).second.value()
-            << std::endl;
+  _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
 }
 
+void Robot::TeleopPeriodic() {}
+
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 165a4b1d..ad231926 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -358,7 +358,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
   }
 }
 
-units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+units::degree_t Vision::LockOn(VisionTargetObjects object) {
   SetMode(VisionModes::kRing);
 
   units::degree_t angle;
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 8bbef0a9..1cb234e1 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -96,7 +96,7 @@ class Vision {
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
   std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
-  units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  units::degree_t LockOn(VisionTargetObjects target);
 
   std::vector<AprilTag> GetTags();