👍

CurtinFRC · Mar 1, 2024 · d0a002c · d0a002c
1 parent 12d2dc1
commit d0a002c
Show file tree

Hide file tree

Showing 3 changed files with 68 additions and 3 deletions.
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
@@ -4,6 +4,9 @@
 
 #include "ShooterBehaviour.h"
 #include "Shooter.h"
+#include "utils/Util.h"
+
+class Vision;
 
 ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester, LED* led)
     : _shooter(shooter), _codriver(tester), _led(led) {
@@ -54,13 +57,66 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 }
 
-VisionShooterSpeed::VisionShooterSpeed(Shooter* shooter)
-    : m_shooter{shooter}, m_table{nt::NetworkTableInstance::GetDefault().GetTable("shooter/visioncontrol")} {
+VisionShooterSpeed::VisionShooterSpeed(Shooter* shooter, Vision* vision)
+    : m_shooter{shooter},
+      m_vision{vision},
+      m_table{nt::NetworkTableInstance::GetDefault().GetTable("shooter/visioncontrol")} {
   Controls(m_shooter);
 }
 
-
 units::meter_t VisionShooterSpeed::DistanceFromTarget() {
+  auto distance = m_vision->GetDistanceToTag();
+  auto tag = m_vision->GetID();
+  switch(tag) {
+    case 1:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 2:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 3:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 4:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 5:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 6:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 7:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 8:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 9:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 10:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 11:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 12:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 13:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 14:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 15:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+    case 16:
+      return wom::utils::Pythagoras(distance, magicvalue);
+    break;
+  }
 }
 
 units::radians_per_second_t VisionShooterSpeed::GetDesiredSpeed(units::meter_t distance) {

diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
@@ -16,6 +16,8 @@
 #include "Shooter.h"
 #include "Wombat.h"
 
+class Vision;
+
 class ShooterManualControl : public behaviour::Behaviour {
  public:
   ShooterManualControl(Shooter* shooter, frc::XboxController* codriver, LED* led);
@@ -44,5 +46,6 @@ class VisionShooterSpeed : public wom::Behaviour {
 
  private:
   Shooter* m_shooter;
+  Vision* m_vision;
   std::shared_ptr<nt::NetworkTable> m_table;
 };
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
@@ -11,6 +11,7 @@
 #include <units/time.h>
 #include <units/voltage.h>
 
+#include <cmath>
 #include <functional>
 #include <iostream>
 #include <memory>
@@ -90,6 +91,11 @@ frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 double deadzone(double val, double deadzone = 0.05);
 double spow2(double val);
 
+template<typename T>
+T Pythagoras(T a, T b) {
+  return (a ^ 2 + b ^ 2) ^ 0.5;
+}
+
 units::volt_t LimitVoltage(units::volt_t voltage);
 units::volt_t GetVoltage(frc::MotorController* controller);
 }  // namespace utils