Merge branch 'gpd' into gpd_merge_temp

src/frc846/include/frc846/math/fieldpoints.h

@@ -44,7 +44,6 @@ struct FieldPoint {
     return mirror(false);
   }
 
-private:
   static constexpr units::inch_t field_size_y = 690.875_in;
   static constexpr units::inch_t field_size_x = 317_in;
 };

src/y2025/cpp/calculators/gpd.cc

@@ -0,0 +1,98 @@
+#include "calculators/gpd.h"
+
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/math.h>
+
+#include <vector>
+
+#include "frc846/robot/swerve/odometry/swerve_pose.h"
+
+GPDCalculator::GPDCalculator() {
+#ifndef _WIN32
+  for (int i = 0; i < 20; i++) {
+    g_field.GetObject(std::to_string(i));
+  }
+  frc::SmartDashboard::PutData("NoteField", &g_field);
+#endif
+}
+
+std::pair<frc846::math::Vector2D, int> GPDCalculator::getBestGP(
+    const std::vector<frc846::math::Vector2D> algae,
+    const frc846::math::VectorND<units::feet_per_second, 2> robot_velocity) {
+  if (algae.empty()) { return {}; }
+  frc846::math::Vector2D closest_algae;
+  int closest_algae_index = -1;
+  double max_dot_product = -std::numeric_limits<double>::infinity();
+  std::pair<double, double> robot_unit_vector = std::pair<double, double>{
+      robot_velocity.unit().toPair().first.to<double>(),
+      robot_velocity.unit().toPair().second.to<double>()};
+
+  for (size_t i = 0; i < algae.size();
+       i++) {  // find closest matching vector in
+               // terms of angle to robot velocity
+    frc846::math::Vector2D relative_note = algae.at(i);
+    std::pair<double, double> note_unit_vector{
+        robot_velocity.unit().toPair().first.to<double>() /
+            relative_note.magnitude().to<double>(),
+        robot_velocity.unit().toPair().second.to<double>() /
+            relative_note.magnitude().to<double>()};
+    double dot_product = robot_unit_vector.first * note_unit_vector.first +
+                         robot_unit_vector.second * note_unit_vector.second;
+    if (dot_product > max_dot_product) {
+      // std::cout << "x: " << relative_note.unit().toPair().first << std::endl;
+      //    std::cout   << "y: " << relative_note.unit().toPair().second <<
+      //    std::endl;
+      max_dot_product = dot_product;
+      closest_algae = relative_note;
+      closest_algae_index = i;
+    }
+  }
+  //   std::cout<<"x"<<closest_note.x.to<double>()<<"y"<<closest_note.y.to<double>()<<std::endl;
+  return std::pair<frc846::math::Vector2D, int>{
+      closest_algae, closest_algae_index};
+}
+
+std::pair<bool, frc846::math::Vector2D> GPDCalculator::calculate(
+    std::vector<frc846::math::Vector2D> gp, std::vector<double> theta_x,
+    frc846::robot::swerve::odometry::SwervePose pose, units::second_t latency,
+    std::vector<double> distances)
+
+{
+  gp.clear();
+
+  units::degree_t bearing_ = pose.bearing;
+  units::inch_t robot_x = pose.position[0];
+  units::inch_t robot_y = pose.position[1];
+  units::feet_per_second_t velocity_x = pose.velocity[0];
+  units::feet_per_second_t velocity_y = pose.velocity[1];
+
+  for (size_t i = 0; i < distances.size() && i < theta_x.size(); ++i) {
+    gp.push_back(
+        frc846::math::Vector2D{units::foot_t(distances[i]),
+            bearing_ + units::degree_t(theta_x[i]), true} +
+        frc846::math::Vector2D{robot_x, robot_y} +
+        frc846::math::Vector2D{velocity_x * latency, velocity_y * latency});
+  }  // Add missing include
+
+  int num_gps = gp.size();
+
+#ifndef _WIN32
+  for (int i = 0; i < std::min(20, num_gps); i++) {
+    auto pos = gp[i];
+    g_field.GetObject(std::to_string(i))->SetPose(pos[0], pos[1], 0_deg);
+  }
+
+#endif
+
+  if (gp.empty()) {
+    return std::pair<bool, frc846::math::Vector2D>{
+        false, frc846::math::Vector2D{}};
+  }
+
+  frc846::math::Vector2D best_algae = getBestGP(gp,
+      frc846::math::VectorND<units::feet_per_second, 2>{velocity_x, velocity_y})
+                                          .first;
+
+  return std::pair<bool, frc846::math::Vector2D>{true, best_algae};
+}

src/y2025/cpp/commands/teleop/drive_command.cc

@@ -1,4 +1,10 @@
 #include "commands/teleop/drive_command.h"
+#include <utility>
+
+
+#include <utility>
+
+#include "subsystems/abstract/gpd.h"
 
 #include "calculators/AntiTippingCalculator.h"
 
@@ -15,6 +21,18 @@ void DriveCommand::Periodic() {
 
   frc846::robot::swerve::DrivetrainOLControlTarget target{};
 
+  bool targeting_algae = container_.control_input_.GetReadings().targeting_algae;
+
+  if (targeting_algae) // add and a note is detected
+  {
+    // Turn towards the note
+    frc846::util::Vector2D rel_note_pos =
+        container_.gpd_.GPDCalculator.algae - 
+        container_.drivetrain_.GetReadings().pose.point;
+    error = container_. - container_.drivetrain_.GetReadings().pose.bearing
+    drivetrain_target.rotation = GetPreferenceValue_double("gpd/kP") * error + GetPreferenceValue_double("gpd/kD") * velocity
+
+  }
   container_.drivetrain_.SetTarget({target});
 
   double translate_x = frc846::math::HorizontalDeadband(

src/y2025/cpp/subsystems/abstract/gpd.cc

@@ -0,0 +1,68 @@
+#include "subsystems/abstract/gpd.h"
+
+#include <vector>
+
+#include "frc846/math/fieldpoints.h"
+
+GPDSubsystem::GPDSubsystem(
+    frc846::robot::swerve::DrivetrainSubsystem* drivetrain)
+    : frc846::robot::GenericSubsystem<GPDReadings, GPDTarget>{"GPD"},
+      drivetrain_{drivetrain} {
+#ifndef _WIN32
+  for (int i = 0; i < 20; i++) {
+    g_field.GetObject(std::to_string(i));
+  }
+  frc::SmartDashboard::PutData("NoteField", &g_field);
+#endif
+}
+
+GPDTarget GPDSubsystem::ZeroTarget() const { return GPDTarget{}; }
+
+bool GPDSubsystem::VerifyHardware() { return true; }
+
+void GPDSubsystem::Setup() {}
+
+GPDReadings GPDSubsystem::ReadFromHardware() {
+  GPDReadings readings;
+  frc846::robot::swerve::DrivetrainReadings drivetrain_readings =
+      drivetrain_->GetReadings();
+
+  g_field.SetRobotPose(frc846::math::FieldPoint::field_size_y -
+                           drivetrain_readings.pose.position[1],
+      drivetrain_readings.pose.position[0],
+      180_deg - drivetrain_readings.pose.bearing);
+
+  std::vector<double> distances = gpdTable->GetNumberArray("distances", {});
+  units::second_t latency = gpdTable->GetNumber("tl", 0.05) *
+                            1_s;  // TODO: check when entry was last updated
+  std::vector<double> theta_x = gpdTable->GetNumberArray("tx", {});
+
+  readings.gamepieces.clear();
+
+  for (size_t i = 0; i < distances.size() && i < theta_x.size(); ++i) {
+    readings.gamepieces.push_back(
+        frc846::math::Vector2D{units::inch_t(distances[i]),
+            drivetrain_readings.pose.bearing + units::degree_t(theta_x[i]),
+            true} +
+        drivetrain_readings.pose.position +
+        frc846::math::Vector2D{drivetrain_readings.pose.velocity[0] * latency,
+            drivetrain_readings.pose.velocity[1] * latency});
+  }
+
+  int num_gps = readings.gamepieces.size();
+
+#ifndef _WIN32
+  for (int i = 0; i < std::min(20, num_gps); i++) {
+    auto pos = readings.gamepieces[i];
+    g_field.GetObject(std::to_string(i))
+        ->SetPose(
+            frc846::math::FieldPoint::field_size_y - pos[1], pos[0], 0_deg);
+  }
+  for (int i = std::min(20, num_gps); i < 20; i++) {
+    g_field.GetObject(std::to_string(i))->SetPose(100_m, 100_m, 0_deg);
+  }
+#endif
+  return readings;
+}
+
+void GPDSubsystem::WriteToHardware(GPDTarget target) {}

src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc

@@ -14,6 +14,10 @@ DrivetrainConstructor::DrivetrainConstructor()
 
   RegisterPreference("drive_motor_voltage_compensation", 16_V);
   RegisterPreference("steer_motor_voltage_compensation", 12_V);
+
+  RegisterPreference("gpd/kP", 12_V);
+  RegisterPreference("gpd/kD", 12_V);
+
 }
 
 frc846::robot::swerve::DrivetrainConfigs

src/y2025/include/calculators/gpd.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <frc/smartdashboard/Field2d.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/math.h>
+
+#include "frc846/base/Loggable.h"
+#include "frc846/math/vectors.h"
+#include "frc846/robot/GenericSubsystem.h"
+#include "frc846/robot/swerve/odometry/swerve_pose.h"
+#include "frc846/wpilib/NTAction.h"
+#include "ports.h"
+
+class GPDCalculator {
+public:
+  GPDCalculator();
+
+  std::pair<frc846::math::Vector2D, int> getBestGP(
+      const std::vector<frc846::math::Vector2D> algae,
+      const frc846::math::VectorND<units::feet_per_second, 2> robot_velocity);
+
+  std::pair<bool, frc846::math::Vector2D> calculate(
+      std::vector<frc846::math::Vector2D> gp, std::vector<double> theta_x,
+      frc846::robot::swerve::odometry::SwervePose pose, units::second_t latency,
+      std::vector<double> distances);
+
+private:
+  frc::Field2d g_field;
+
+  frc846::base::Loggable algo_params{"algo_parameters"};
+
+  std::shared_ptr<nt::NetworkTable> gpdTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("GPDCam1");
+};

src/y2025/include/subsystems/abstract/control_input.h

@@ -21,6 +21,9 @@ struct ControlInputReadings {
   bool lock_right_reef;
 
   bool zero_bearing;
+
+
+  bool targeting_algae;
 };
 
 struct ControlInputTarget {

src/y2025/include/subsystems/abstract/gpd.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/math.h>
+
+#include "frc846/base/Loggable.h"
+#include "frc846/math/vectors.h"
+#include "frc846/robot/GenericSubsystem.h"
+#include "frc846/robot/swerve/drivetrain.h"
+#include "ports.h"
+
+struct GPDTarget {};
+
+struct GPDReadings {
+  std::vector<frc846::math::Vector2D> gamepieces;
+};
+
+class GPDSubsystem
+    : public frc846::robot::GenericSubsystem<GPDReadings, GPDTarget> {
+public:
+  GPDSubsystem(frc846::robot::swerve::DrivetrainSubsystem* drivetrain);
+
+  void Setup() override;
+
+  GPDTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+private:
+  frc::Field2d g_field;
+
+  std::shared_ptr<nt::NetworkTable> gpdTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("GPDCam1");
+
+  GPDReadings ReadFromHardware() override;
+
+  void WriteToHardware(GPDTarget target) override;
+
+  frc846::robot::swerve::DrivetrainSubsystem* drivetrain_;
+};

src/y2025/include/subsystems/robot_container.h

@@ -2,6 +2,7 @@
 
 #include "frc846/robot/GenericRobotContainer.h"
 #include "subsystems/abstract/control_input.h"
+#include "subsystems/abstract/gpd.h"
 #include "subsystems/hardware/DrivetrainConstructor.h"
 #include "subsystems/hardware/leds.h"
 
@@ -14,16 +15,20 @@ class RobotContainer : public frc846::robot::GenericRobotContainer {
   frc846::robot::swerve::DrivetrainSubsystem drivetrain_{
       drivetrain_constructor_.getDrivetrainConfigs()};
 
+  GPDSubsystem GPD_{&drivetrain_};
+
   RobotContainer() {
     RegisterPreference("init_drivetrain", true);
     RegisterPreference("init_leds", true);
+    RegisterPreference("init_gpd", true);
 
-    control_input_.Init();
     bool drivetrain_init = (GetPreferenceValue_bool("init_drivetrain"));
     bool leds_init = (GetPreferenceValue_bool("init_leds"));
+    bool gpd_init = (GetPreferenceValue_bool("init_gpd"));
 
     RegisterSubsystemGroupA({{&control_input_, true}});
-    RegisterSubsystemGroupB({{&leds_, leds_init}});
     RegisterSubsystemGroupAB({{&drivetrain_, drivetrain_init}});
+    RegisterSubsystemGroupB({{&leds_, leds_init}});
+    RegisterSubsystemGroupAB({{&GPD_, gpd_init}});
   }
 };