more cracked code

CurtinFRC · Mar 6, 2024 · 9696ed9 · 9696ed9
1 parent cedd611
commit 9696ed9
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 59 deletions.
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
@@ -59,73 +59,21 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 }
 
-VisionShooterSpeed::VisionShooterSpeed(Shooter* shooter, Vision* vision)
+VisionShooterSpeed::VisionShooterSpeed(Shooter* shooter, Vision* vision, wom::SwerveDrive* swerve)
     : m_shooter{shooter},
       m_vision{vision},
+      m_swerve{swerve},
       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:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 2:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      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:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 10:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 11:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 12:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 13:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 14:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 15:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-    case 16:
-      throw new std::runtime_error("UNSUPPORTED TAG");
-      break;
-  }
-}
-
 units::radians_per_second_t VisionShooterSpeed::GetDesiredSpeed(units::meter_t distance) {
   return units::radians_per_second_t{0};
 }
 
 void VisionShooterSpeed::OnTick(units::second_t dt) {
+  auto tag = m_vision->GetTag();
+  m_vision->TurnToTarget(tag, m_swerve);
   m_shooter->SetState(ShooterState::kSpinUp);
-  m_shooter->SetPidGoal(GetDesiredSpeed(DistanceFromTarget()));
+  m_shooter->SetPidGoal(GetDesiredSpeed(m_vision->GetDistance(tag)));
 }
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
@@ -36,15 +36,15 @@ class ShooterManualControl : public behaviour::Behaviour {
 
 class VisionShooterSpeed : public wom::Behaviour {
  public:
-  VisionShooterSpeed(Shooter* shooter, Vision* vision);
+  VisionShooterSpeed(Shooter* shooter, Vision* vision, wom::SwerveDrive* swerve);
 
-  units::meter_t DistanceFromTarget();
   units::radians_per_second_t GetDesiredSpeed(units::meter_t distance);
 
   void OnTick(units::second_t dt) override;
 
  private:
   Shooter* m_shooter;
   Vision* m_vision;
+  wom::SwerveDrive* m_swerve;
   std::shared_ptr<nt::NetworkTable> m_table;
 };