diff --git a/.vscode/settings.json b/.vscode/settings.json index 8fa1620..421b6d8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -114,7 +114,9 @@ "xtr1common": "cpp", "xtree": "cpp", "xutility": "cpp", - "queue": "cpp" + "queue": "cpp", + "*.clang-format": "cpp", + "expected": "cpp" }, "wpilib.useWindbgX": true, "C_Cpp.errorSquiggles": "enabled", diff --git a/src/y2025/cpp/subsystems/abstract/gpd.cc b/src/y2025/cpp/subsystems/abstract/gpd.cc index d65bc09..9d4478d 100644 --- a/src/y2025/cpp/subsystems/abstract/gpd.cc +++ b/src/y2025/cpp/subsystems/abstract/gpd.cc @@ -1,47 +1,91 @@ #include "subsystems/abstract/gpd.h" -//#include "subsystems/SubsystemHelper.h" - -GPDSubsystem::GPDSubsystem() - : frc846::robot::GenericSubsystem( - "gpd"), - motor_configs(GET_MOTOR_CONFIG("gpd/gpd_one_", - ports::gpd_::kGPDOne_CANID, frc846::wpilib::unit_ohm{0.0}, - frc846::wpilib::unit_kg_m_sq{0.0})), - gpd_(frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60, - motor_configs) { - RegisterPreference("gpd_tolerance_", 0.25_in); - - -} +#include +#include + +#include -void GPDSubsystem::Setup() { - gpd_.Setup(); - gpd.EnableStatusFrames( - {frc846::control::config::StatusFrame::kPositionFrame, - frc846::control::config::StatusFrame::kVelocityFrame}); - motor_helper_.SetPosition(0.0_in); +#include "field.h" +#include "frc846/ntinf/pref.h" +#include "frc846/util/share_tables.h" + +GPDSubsystem::GPDSubsystem(bool init) + : frc846::robot::GenericSubsystem("gpd", init) { + if (init) { +#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{0.0_in}; + GPDTarget target; + return target; } -bool GPDSubsystem::VerifyHardware() { - bool ok = true; - FRC846_VERIFY( - gpd_.VerifyConnected(), ok, "Could not verify gpd motor"); - return ok; +bool GPDSubsystem::VerifyHardware() { return true; } + +frc846::math::Vector2D GPDSubsystem::findDistance(units::degree_t theta_h, + units::degree_t theta_v) { + units::foot_t height = mount_height_.value() - algae_height_.value(); + auto dist = units::math::abs(height * units::math::tan(theta_v)); + auto yDist = dist * units::math::cos(theta_h); + auto xDist = dist * units::math::sin(theta_h); + + return {xDist, yDist}; } -ElevatorReadings ElevatorSubsystem::ReadFromHardware() { - ElevatorReadings readings; - readings.height = motor_helper_.GetPosition(); - Graph("readings/elevator_position", readings.height); +GPDReadings GPDSubsystem::ReadFromHardware() { + GPDReadings readings{}; + + units::degree_t bearing_ = + units::degree_t(GetPreferenceValue_double("robot_bearing_")); + + units::inch_t robot_x = + units::foot_t(GetPreferenceValue_double("odometry_x_")); + + units::inch_t robot_y = + units::foot_t(GetPreferenceValue_double("odometry_y_")); + + units::feet_per_second_t velocity_x = units::feet_per_second_t( + GetPreferenceValue_double("velocity_x_")); + + units::feet_per_second_t velocity_y = units::feet_per_second_t( + GetPreferenceValue_double("velocity_y_")); + + std::vector theta_hs = gpdTable->GetNumberArray("theta_h", {}); + std::vector theta_vs = gpdTable->GetNumberArray("theta_v", {}); + auto latency = gpdTable->GetNumber("latency", 0.1) * 1_s; + + for (size_t i = 0; i < theta_hs.size(); i++) { + auto distance = findDistance(units::degree_t(theta_hs[i]), + units::degree_t(theta_vs[i])); + auto pos = + distance.rotate(bearing_) + frc846::math::Vector2D{robot_x, robot_y} + + frc846::math::Vector2D{velocity_x * latency, velocity_y * latency}; + + readings.notes.push_back(pos); + } + + int num_notes = readings.notes.size(); + Graph("notes_detected", num_notes); + +#ifndef _WIN32 + for (int i = 0; i < std::min(20, num_notes); i++) { + auto pos = readings.notes[i]; + g_field.getPoint(std::to_string(i)) + ->SetPose(pos[0], pos[1], frc::Rotation2d(0_deg)); + } + for (int i = std::min(20, num_notes); i < 20; i++) { + g_field.GetObject(std::to_string(i)) + ->SetPose(5000_ft, 5000_ft, frc::Rotation2d(0_deg)); + } +#endif + return readings; } -void ElevatorSubsystem::WriteToHardware(ElevatorTarget target) { - elevator_.SetGains(GET_PIDF_GAINS("elevator/elevator_gains_")); - motor_helper_.WritePosition(target.height); -} \ No newline at end of file +void GPDSubsystem::WriteToHardware(GPDTarget target) {} \ No newline at end of file diff --git a/src/y2025/include/subsystems/abstract/gpd.h b/src/y2025/include/subsystems/abstract/gpd.h index bca3e9a..4fffdbd 100644 --- a/src/y2025/include/subsystems/abstract/gpd.h +++ b/src/y2025/include/subsystems/abstract/gpd.h @@ -2,27 +2,28 @@ #include #include -#include +#include #include "frc/smartdashboard/Smartdashboard.h" -#include "frc846/ntinf/grapher.h" #include "frc846/robot/GenericSubsystem.h" -#include "frc846/util/math.h" -#include "networktables/NetworkTable.h" -#include "networktables/NetworkTableEntry.h" -#include "networktables/NetworkTableInstance.h" -#include "networktables/NetworkTableValue.h" +#include "frc846/math/constants.h" +#include "frc846/math/vectors.h" +#include "frc846/math/collection.h" +#include "frc846/base/Loggable.h" +#include "frc846/wpilib/NTAction.h" +#include "field.h" + #include "ports.h" -#include "subsystems/hardware/drivetrain.h" struct GPDTarget {}; + struct GPDReadings { bool note_detected; units::degree_t note_angle; - std::vector> points; + std::vector notes; int note_index; - frc846::util::Vector2D closest_note; + frc846::math::Vector2D closest_note; }; class GPDSubsystem @@ -30,82 +31,39 @@ class GPDSubsystem public: GPDSubsystem(bool init); - void Setup() override {}; - - std::pair, int> getBestNote( - const std::vector>& notes, - const frc846::util::Vector2D& robot_velocity); + std::pair getBestNote( + const std::vector& notes, + const frc846::math::Vector2D& robot_velocity); + + frc846::math::Vector2D findDistance( + units::degree_t theta_h, units::degree_t theta_v); - frc846::util::Vector2D findDistance(units::degree_t theta_h, - units::degree_t theta_v); + void Setup() override {}; GPDTarget ZeroTarget() const override; bool VerifyHardware() override; - private: - GPDReadings prevReadings; - - frc846::util::Position poseAtLastRequest; - bool requested = false; - int prevFrame = -1; - - frc846::base::Loggable readings_named{*this, "readings"}; - frc846::ntinf::Grapher note_detected_graph_{readings_named, - "note_detected"}; - - frc846::base::Loggable algo_params{*this, "algo_parameters"}; - frc846::ntinf::Pref mount_height_{algo_params, "mount_height", - 1_ft}; - frc846::ntinf::Pref note_height_{algo_params, "note_height", - 0_ft}; - frc846::ntinf::Pref nt_latency{algo_params, "nt_latency", - 0_s}; - - std::shared_ptr gpdTable = - nt::NetworkTableInstance::GetDefault().GetTable("gpd"); - frc846::ntinf::Grapher note_distance_magnitude_graph_{ - readings_named, "note_distance_magnitude_graph"}; - frc846::ntinf::Grapher note_angle_graph_{readings_named, - "note_angle_graph"}; - frc846::ntinf::Grapher total_latency_{readings_named, - "total_latency_graph"}; - - nt::NetworkTableInstance nt_table = nt::NetworkTableInstance::GetDefault(); - - std::shared_ptr raspiPreferences = - nt::NetworkTableInstance::GetDefault().GetTable("RaspiPreferences"); - GPDReadings ReadFromHardware() override; void WriteToHardware(GPDTarget target) override; -}; - -public: - GPDSubsystem(); + // bool WithinTolerance(units::inch_t pos) {} - void Setup() override; - - ElevatorTarget ZeroTarget() const override; - - bool VerifyHardware() override; +private: + Field g_field; + GPDReadings prevReadings; - bool WithinTolerance(units::inch_t pos) { - return units::math::abs(pos - GetReadings().height) < - GetPreferenceValue_unit_type("elevator_tolerance_"); - } + frc846::base::Loggable readings_named{*this, "readings"}; + frc846::base::Loggable algo_params{"ago_parameters"}; -private: - bool hasZeroed = false; - // TODO: Set to correct reduction later - elevator_pos_conv_t elevator_reduction_ = 1.0_in / 1.0_tr; - frc846::control::config::MotorConstructionParameters motor_configs; - frc846::control::HigherMotorController elevator_; - frc846::control::HMCHelper motor_helper_; + units::foot_t mount_height_ = GetPreferenceValue_unit_type("gpd_mount_height"); + units::foot_t algae_height_ = GetPreferenceValue_unit_type("gpd_algae_height"); + units::second_t latency_ = GetPreferenceValue_unit_type("gpd_latency"); + + std::shared_ptr gpdTable = + nt::NetworkTableInstance::GetDefault().GetTable("gpd"); - ElevatorReadings ReadFromHardware() override; - void WriteToHardware(ElevatorTarget target) override; }; \ No newline at end of file