vision speed tracking

src/main/cpp/Robot.cpp

@@ -19,7 +19,6 @@
 
 #include "RobotMap.h"
 #include "behaviour/HasBehaviour.h"
-#include "networktables/NetworkTableInstance.h"
 
 static units::second_t lastPeriodic;
 
@@ -175,28 +174,5 @@ void Robot::DisabledPeriodic() {}
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
-void Robot::SimulationInit() {
-  /*  std::string x = "[";
-    std::string y = "[";
-    // _vision->GetDistanceToTarget(16);
-    for (int i = 1; i < 17; i++) {
-      for (int j = 0; j < 17; j++) {
-        frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
-        x += std::to_string(pose.X().value()) + ",";
-        y += std::to_string(pose.Y().value()) + ",";
-      }
-    }
-
-    x += "]";
-    y += "]";
-
-    std::cout << x << std::endl;
-    std::cout << y << std::endl; */
-  // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
-  // Reimplement when vision is reimplemented
-  // frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
-  // nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot",
-  //                                                                     pose.Rotation().Degrees().value());
-}
-
+void Robot::SimulationInit() {}
 void Robot::SimulationPeriodic() {}

src/main/cpp/ShooterBehaviour.cpp

@@ -3,6 +3,12 @@
 // of the MIT License at the root of this project
 
 #include "ShooterBehaviour.h"
+#include <stdexcept>
+
+#include "Shooter.h"
+#include "utils/Util.h"
+
+class Vision;
 
 ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester, LED* led)
     : _shooter(shooter), _codriver(tester), _led(led) {
@@ -52,3 +58,22 @@ void ShooterManualControl::OnTick(units::second_t dt) {
     }
   }
 }
+
+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::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(m_vision->GetDistance(tag)));
+}

src/main/include/ShooterBehaviour.h

@@ -5,13 +5,18 @@
 #pragma once
 
 #include <frc/XboxController.h>
+#include <networktables/NetworkTable.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
 
 #include <memory>
 
 #include "LED.h"
 #include "Shooter.h"
 #include "Wombat.h"
 
+class Vision;
+
 class ShooterManualControl : public behaviour::Behaviour {
  public:
   ShooterManualControl(Shooter* shooter, frc::XboxController* codriver, LED* led);
@@ -28,3 +33,18 @@ class ShooterManualControl : public behaviour::Behaviour {
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
 };
+
+class VisionShooterSpeed : public wom::Behaviour {
+ public:
+  VisionShooterSpeed(Shooter* shooter, Vision* vision, wom::SwerveDrive* swerve);
+
+  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;
+};

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>
@@ -89,6 +90,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