Did more vision stuff

CurtinFRC · Feb 12, 2024 · 87c92ed · 87c92ed
1 parent bf9b1b4
commit 87c92ed
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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();