diff --git a/src/y2024/cpp/calculators/april_tag_calculator.cc b/src/y2024/cpp/calculators/april_tag_calculator.cc new file mode 100644 index 0000000..5fd6457 --- /dev/null +++ b/src/y2024/cpp/calculators/april_tag_calculator.cc @@ -0,0 +1,38 @@ +#include "calculators/april_tag_calculator.h" + +ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) { + if ((input.distances.size() == input.thetas.size() && + input.thetas.size() == input.tags.size())) { + frc846::math::VectorND pos; + int succesfulTags = 0; + for (int i = 0; i < input.distances.size(); i++) { + if (constants_.tag_locations.contains(input.tags[i])) { + pos += getPos(input.bearing, input.thetas[i], input.distances[i], + input.tags[i]); + succesfulTags++; + } + } + pos /= succesfulTags; + if (succesfulTags != 0) { + return {pos}; + } + } + + return {{-1_ft, -1_ft}}; +} + +frc846::math::VectorND AprilTagCalculator::getPos( + units::degree_t bearing, units::degree_t theta, units::inch_t distance, + int tag) { + frc846::math::VectorND local_tag_pos{ + -distance * units::math::sin(theta + constants_.cam_angle_offset) - + constants_.camera_x_offset, + distance * units::math::cos(theta + constants_.cam_angle_offset) + + constants_.camera_y_offset}; + local_tag_pos = local_tag_pos.rotate(bearing); + std::cout << local_tag_pos[0].to() << std::endl; + return { + constants_.tag_locations[tag].x_pos - local_tag_pos[0], + constants_.tag_locations[tag].y_pos - local_tag_pos[1], + }; +} \ No newline at end of file diff --git a/src/y2024/cpp/subsystems/abstract/vision.cc b/src/y2024/cpp/subsystems/abstract/vision.cc index 060e583..a628445 100644 --- a/src/y2024/cpp/subsystems/abstract/vision.cc +++ b/src/y2024/cpp/subsystems/abstract/vision.cc @@ -25,153 +25,162 @@ VisionTarget VisionSubsystem::ZeroTarget() const { bool VisionSubsystem::VerifyHardware() { return true; } VisionReadings VisionSubsystem::ReadFromHardware() { - if (auto alliance = frc::DriverStation::GetAlliance()) { - if (alliance.value() == frc::DriverStation::Alliance::kRed) { - frc846::util::ShareTables::SetBoolean("is_red_side", true); - } - if (alliance.value() == frc::DriverStation::Alliance::kBlue) { - frc846::util::ShareTables::SetBoolean("is_red_side", false); - } - } else { - frc846::util::ShareTables::SetBoolean("is_red_side", - default_is_red_side_.value()); - } + // if (auto alliance = frc::DriverStation::GetAlliance()) { + // if (alliance.value() == frc::DriverStation::Alliance::kRed) { + // frc846::util::ShareTables::SetBoolean("is_red_side", true); + // } + // if (alliance.value() == frc::DriverStation::Alliance::kBlue) { + // frc846::util::ShareTables::SetBoolean("is_red_side", false); + // } + // } else { + // frc846::util::ShareTables::SetBoolean("is_red_side", + // default_is_red_side_.value()); + // } VisionReadings newReadings{}; - double targetOffsetAngle_Horizontal = table->GetNumber("tx", 0.0); - double targetOffsetAngle_Vertical = table->GetNumber("ty", 0.0); - double tag_id = table->GetNumber("tid", 0.0); + // double targetOffsetAngle_Horizontal = table->GetNumber("tx", 0.0); + // double targetOffsetAngle_Vertical = table->GetNumber("ty", 0.0); + // double tag_id = table->GetNumber("tid", 0.0); - tag_id_graph_.Graph(tag_id); + // tag_id_graph_.Graph(tag_id); - double latency = table->GetNumber("tl", 0.0) + table->GetNumber("cl", 0.0); + // double latency = table->GetNumber("tl", 0.0) + table->GetNumber("cl", + // 0.0); - ll_latency_graph_.Graph(latency * 1_s); + // ll_latency_graph_.Graph(latency * 1_s); - units::degree_t bearing_ = - units::degree_t(frc846::util::ShareTables::GetDouble("robot_bearing_")); + // units::degree_t bearing_ = + // units::degree_t(frc846::util::ShareTables::GetDouble("robot_bearing_")); - units::degrees_per_second_t bearing_velocity = units::degrees_per_second_t( - frc846::util::ShareTables::GetDouble("bearing_velocity")); + // units::degrees_per_second_t bearing_velocity = + // units::degrees_per_second_t( + // frc846::util::ShareTables::GetDouble("bearing_velocity")); - units::second_t latency_seconds{latency}; + // units::second_t latency_seconds{latency}; - bearing_ = bearing_ - bearing_velocity * latency_seconds; + // bearing_ = bearing_ - bearing_velocity * latency_seconds; - units::inch_t robot_x = - units::foot_t(frc846::util::ShareTables::GetDouble("odometry_x_")); + // units::inch_t robot_x = + // units::foot_t(frc846::util::ShareTables::GetDouble("odometry_x_")); - units::inch_t robot_y = - units::foot_t(frc846::util::ShareTables::GetDouble("odometry_y_")); + // units::inch_t robot_y = + // units::foot_t(frc846::util::ShareTables::GetDouble("odometry_y_")); - units::feet_per_second_t velocity_x = units::feet_per_second_t( - frc846::util::ShareTables::GetDouble("velocity_x_")); + // units::feet_per_second_t velocity_x = units::feet_per_second_t( + // frc846::util::ShareTables::GetDouble("velocity_x_")); - units::feet_per_second_t velocity_y = units::feet_per_second_t( - frc846::util::ShareTables::GetDouble("velocity_y_")); + // units::feet_per_second_t velocity_y = units::feet_per_second_t( + // frc846::util::ShareTables::GetDouble("velocity_y_")); - if (tag_locations.contains(tag_id)) { // && - // frc846::util::ShareTables::GetString("mode").compare("kTeleop") == 0) { - tag_in_sight_graph_.Graph(true); + // if (tag_locations.contains(tag_id)) { // && + // // frc846::util::ShareTables::GetString("mode").compare("kTeleop") == + // 0) { tag_in_sight_graph_.Graph(true); - auto april_tag = tag_locations[tag_id]; - units::inch_t height_difference_ = - april_tag.height - camera_height_.value(); + // auto april_tag = tag_locations[tag_id]; + // units::inch_t height_difference_ = + // april_tag.height - camera_height_.value(); - units::degree_t april_tag_vertical_angle = - units::degree_t(targetOffsetAngle_Vertical) + camera_angle_.value(); + // units::degree_t april_tag_vertical_angle = + // units::degree_t(targetOffsetAngle_Vertical) + + // camera_angle_.value(); - newReadings.tag_distance = - height_difference_ / units::math::tan(april_tag_vertical_angle); + // newReadings.tag_distance = + // height_difference_ / units::math::tan(april_tag_vertical_angle); - newReadings.local_y_dist = - newReadings.tag_distance * - units::math::cos(units::degree_t(targetOffsetAngle_Horizontal) + - 180_deg) + - camera_y_offset_.value(); - newReadings.local_x_dist = - newReadings.tag_distance * - units::math::sin(units::degree_t(targetOffsetAngle_Horizontal) + - 180_deg) + - camera_x_offset_.value(); + // newReadings.local_y_dist = + // newReadings.tag_distance * + // units::math::cos(units::degree_t(targetOffsetAngle_Horizontal) + // + + // 180_deg) + + // camera_y_offset_.value(); + // newReadings.local_x_dist = + // newReadings.tag_distance * + // units::math::sin(units::degree_t(targetOffsetAngle_Horizontal) + // + + // 180_deg) + + // camera_x_offset_.value(); - robot_y += can_bus_latency_.value() * velocity_y; + // robot_y += can_bus_latency_.value() * velocity_y; - robot_x += can_bus_latency_.value() * velocity_x; + // robot_x += can_bus_latency_.value() * velocity_x; - // auto flipped = frc846::util::FieldPoint::flip( - // {{robot_x, robot_y}, bearing_}, - // !frc846::util::ShareTables::GetBoolean("is_red_side"), true); + // // auto flipped = frc846::util::FieldPoint::flip( + // // {{robot_x, robot_y}, bearing_}, + // // !frc846::util::ShareTables::GetBoolean("is_red_side"), true); - // robot_y = flipped.point[1]; - // robot_x = flipped.point[0]; - // bearing_ = flipped.bearing; + // // robot_y = flipped.point[1]; + // // robot_x = flipped.point[0]; + // // bearing_ = flipped.bearing; - frc846::math::VectorND local_tag_vec{ - newReadings.local_x_dist, newReadings.local_y_dist}; - auto tag_vec = local_tag_vec.rotate(bearing_); + // frc846::math::VectorND local_tag_vec{ + // newReadings.local_x_dist, newReadings.local_y_dist}; + // auto tag_vec = local_tag_vec.rotate(bearing_); - units::inch_t tag_x_dist = tag_vec[0]; + // units::inch_t tag_x_dist = tag_vec[0]; - units::inch_t tag_y_dist = tag_vec[1]; + // units::inch_t tag_y_dist = tag_vec[1]; - y_correction = - (april_tag.y_pos - tag_y_dist) - (robot_y - latency * 1_s * velocity_y); + // y_correction = + // (april_tag.y_pos - tag_y_dist) - (robot_y - latency * 1_s * + // velocity_y); - x_correction = - (april_tag.x_pos - tag_x_dist) - (robot_x - latency * 1_s * velocity_x); - } else { - tag_in_sight_graph_.Graph(false); - } + // x_correction = + // (april_tag.x_pos - tag_x_dist) - (robot_x - latency * 1_s * + // velocity_x); + // } else { + // tag_in_sight_graph_.Graph(false); + // } - robot_y += can_bus_latency_.value() * velocity_y + y_correction; + // robot_y += can_bus_latency_.value() * velocity_y + y_correction; - robot_x += can_bus_latency_.value() * velocity_x + x_correction; + // robot_x += can_bus_latency_.value() * velocity_x + x_correction; - newReadings.x_pos = robot_x; - newReadings.y_pos = robot_y; + // newReadings.x_pos = robot_x; + // newReadings.y_pos = robot_y; - robot_x_graph_.Graph(robot_x); - robot_y_graph_.Graph(robot_y); + // robot_x_graph_.Graph(robot_x); + // robot_y_graph_.Graph(robot_y); - auto speaker_pt = - Field::getPoint("kSpeaker") - .mirrorOnlyY(!frc846::util::ShareTables::GetBoolean("is_red_side")); + // auto speaker_pt = + // Field::getPoint("kSpeaker") + // .mirrorOnlyY(!frc846::util::ShareTables::GetBoolean("is_red_side")); - newReadings.est_dist_from_speaker_x = robot_x - speaker_pt.point[0]; + // newReadings.est_dist_from_speaker_x = robot_x - speaker_pt.point[0]; - newReadings.est_dist_from_speaker_y = robot_y - speaker_pt.point[1]; + // newReadings.est_dist_from_speaker_y = robot_y - speaker_pt.point[1]; - auto point_target = frc846::math::VectorND{ - -newReadings.est_dist_from_speaker_x, - -newReadings.est_dist_from_speaker_y}; + // auto point_target = frc846::math::VectorND{ + // -newReadings.est_dist_from_speaker_x, + // -newReadings.est_dist_from_speaker_y}; - newReadings.velocity_in_component = - ((velocity_x * point_target[0] + velocity_y * point_target[1]) / - point_target.magnitude()) - .to(); + // newReadings.velocity_in_component = + // ((velocity_x * point_target[0] + velocity_y * point_target[1]) / + // point_target.magnitude()) + // .to(); - point_target = point_target.rotate(90_deg); + // point_target = point_target.rotate(90_deg); - newReadings.velocity_orth_component = - ((velocity_x * point_target[0] + velocity_y * point_target[1]) / - point_target.magnitude()) - .to(); + // newReadings.velocity_orth_component = + // ((velocity_x * point_target[0] + velocity_y * point_target[1]) / + // point_target.magnitude()) + // .to(); - newReadings.est_dist_from_speaker = units::math::sqrt( - units::math::pow<2, units::foot_t>(newReadings.est_dist_from_speaker_x) + - units::math::pow<2, units::foot_t>(newReadings.est_dist_from_speaker_y)); + // newReadings.est_dist_from_speaker = units::math::sqrt( + // units::math::pow<2, + // units::foot_t>(newReadings.est_dist_from_speaker_x) + + // units::math::pow<2, + // units::foot_t>(newReadings.est_dist_from_speaker_y)); - speaker_x_dist_graph_.Graph(newReadings.est_dist_from_speaker_x); - speaker_y_dist_graph_.Graph(newReadings.est_dist_from_speaker_y); - speaker_dist_graph_.Graph(newReadings.est_dist_from_speaker); - local_x_dist_graph_.Graph(newReadings.local_x_dist); - local_y_dist_graph_.Graph(newReadings.local_y_dist); - tag_distance_graph_.Graph(newReadings.tag_distance); - tag_angle_difference_graph_.Graph(newReadings.tag_angle_difference); - velocity_in_component_graph_.Graph(newReadings.velocity_in_component); - velocity_orth_component_graph_.Graph(newReadings.velocity_orth_component); + // speaker_x_dist_graph_.Graph(newReadings.est_dist_from_speaker_x); + // speaker_y_dist_graph_.Graph(newReadings.est_dist_from_speaker_y); + // speaker_dist_graph_.Graph(newReadings.est_dist_from_speaker); + // local_x_dist_graph_.Graph(newReadings.local_x_dist); + // local_y_dist_graph_.Graph(newReadings.local_y_dist); + // tag_distance_graph_.Graph(newReadings.tag_distance); + // tag_angle_difference_graph_.Graph(newReadings.tag_angle_difference); + // velocity_in_component_graph_.Graph(newReadings.velocity_in_component); + // velocity_orth_component_graph_.Graph(newReadings.velocity_orth_component); return newReadings; } diff --git a/src/y2024/cpp/subsystems/hardware/drivetrain.cc b/src/y2024/cpp/subsystems/hardware/drivetrain.cc index e5535aa..4039347 100644 --- a/src/y2024/cpp/subsystems/hardware/drivetrain.cc +++ b/src/y2024/cpp/subsystems/hardware/drivetrain.cc @@ -14,6 +14,9 @@ DrivetrainSubsystem::DrivetrainSubsystem(bool initialize) bearing_offset_ = 0_deg; ZeroOdometry(); frc::SmartDashboard::PutData("Field", &m_field); + april_calc.setConstants({april_locations, camera_x_offset_.value(), + camera_y_offset_.value(), + cam_angle_offset_.value()}); } void DrivetrainSubsystem::ZeroModules() { @@ -177,6 +180,44 @@ bool DrivetrainSubsystem::VerifyHardware() { return ok; } +DrivetrainReadings DrivetrainSubsystem::trackTags(DrivetrainReadings input) { + units::second_t tl = units::second_t(april_table->GetNumber("tl", -1)); + std::vector tx_num = april_table->GetNumberArray("tx", {}); + std::vector distances_num = + april_table->GetNumberArray("distances", {}); + std::vector tx; + std::vector distances; + for (double tx_num : tx_num) { + tx.push_back(units::degree_t(tx_num)); + }; + for (double distance_num : distances_num) { + distances.push_back(units::inch_t(distance_num)); + }; + std::vector tags = april_table->GetNumberArray("tags", {}); + + units::degree_t bearingAtCapture = + input.pose.bearing - input.angular_velocity * tl; + ATCalculatorOutput out = + april_calc.calculate({bearingAtCapture, tx, distances, tags}); + + DrivetrainReadings readings = input; + readings.april_pose = readings.pose; + if (out.pos[0] == -1_ft && out.pos[1] == -1_ft) { + readings.april_pose.point = readings.pose.point - GetReadings().pose.point + + GetReadings().april_pose.point; + } else { + readings.april_pose.point = { + out.pos[0] + input.pose.velocity[0] * (tl + fudge_latency_.value()), + out.pos[1] + input.pose.velocity[1] * (tl + fudge_latency_.value()), + }; + } + + april_x_graph_.Graph(readings.april_pose.point[0]); + april_y_graph_.Graph(readings.april_pose.point[1]); + + return readings; +} + DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { DrivetrainReadings readings{}; @@ -220,8 +261,9 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { frc846::math::VectorND unfiltered_velocity = { total_x / kModuleCount, total_y / kModuleCount}; - readings.pose = frc846::math::FieldPoint(odometry_.position(), bearing, - unfiltered_velocity); + readings.pose = {odometry_.position(), bearing, unfiltered_velocity}; + + readings = trackTags(readings); pose_x_graph_.Graph(odometry_.position()[0]); pose_y_graph_.Graph(odometry_.position()[1]); diff --git a/src/y2024/include/calculators/april_tag_calculator.h b/src/y2024/include/calculators/april_tag_calculator.h new file mode 100644 index 0000000..8c089c8 --- /dev/null +++ b/src/y2024/include/calculators/april_tag_calculator.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "frc846/math/calculator.h" +#include "frc846/math/vectors.h" +#include "frc846/ntinf/pref.h" + +struct ATCalculatorInput { + units::degree_t bearing; + std::vector thetas; + std::vector distances; + std::vector tags; +}; + +struct ATCalculatorOutput { + frc846::math::VectorND pos; +}; + +struct AprilTagData { + units::inch_t x_pos; + units::inch_t y_pos; + units::degree_t angle; + units::inch_t height; +}; + +struct ATCalculatorConstants { + std::map tag_locations; + units::inch_t camera_x_offset; + units::inch_t camera_y_offset; + units::degree_t cam_angle_offset; +}; + +class AprilTagCalculator + : public frc846::math::Calculator { + public: + AprilTagCalculator() {}; + + ATCalculatorOutput calculate(ATCalculatorInput input) override; + frc846::math::VectorND getPos(units::degree_t bearing, + units::degree_t theta, + units::inch_t distance, + int tag); +}; \ No newline at end of file diff --git a/src/y2024/include/subsystems/abstract/vision.h b/src/y2024/include/subsystems/abstract/vision.h index 208828a..ce29636 100644 --- a/src/y2024/include/subsystems/abstract/vision.h +++ b/src/y2024/include/subsystems/abstract/vision.h @@ -29,12 +29,12 @@ struct VisionReadings { struct VisionTarget {}; -struct AprilTagData { - units::inch_t x_pos; - units::inch_t y_pos; - units::degree_t angle; - units::inch_t height; -}; +// struct AprilTagData { +// units::inch_t x_pos; +// units::inch_t y_pos; +// units::degree_t angle; +// units::inch_t height; +// }; class VisionSubsystem : public frc846::robot::GenericSubsystem { @@ -51,62 +51,67 @@ class VisionSubsystem false}; private: - frc846::ntinf::Pref default_is_red_side_{*this, "default_is_red_side"}; - - frc846::ntinf::Grapher tag_in_sight_graph_{*this, "tag_in_sight"}; - frc846::ntinf::Grapher tag_id_graph_{*this, "tag_id"}; - frc846::ntinf::Grapher ll_latency_graph_{*this, - "ll_latency"}; - - frc846::base::Loggable readings_named{*this, "readings"}; - frc846::ntinf::Grapher robot_x_graph_{readings_named, - "robot_x"}; - frc846::ntinf::Grapher robot_y_graph_{readings_named, - "robot_y"}; - frc846::ntinf::Grapher speaker_x_dist_graph_{readings_named, - "speaker_dist_x"}; - frc846::ntinf::Grapher speaker_y_dist_graph_{readings_named, - "speaker_dist_y"}; - frc846::ntinf::Grapher speaker_dist_graph_{readings_named, - "speaker_dist"}; - frc846::ntinf::Grapher local_x_dist_graph_{readings_named, - "local_dist_x"}; - frc846::ntinf::Grapher local_y_dist_graph_{readings_named, - "local_dist_y"}; - frc846::ntinf::Grapher tag_distance_graph_{readings_named, - "tag_distance"}; - frc846::ntinf::Grapher tag_angle_difference_graph_{ - readings_named, "tag_angle_difference"}; - - frc846::ntinf::Grapher velocity_in_component_graph_{ - readings_named, "velocity_in_component"}; - frc846::ntinf::Grapher velocity_orth_component_graph_{ - readings_named, "velocity_orth_component"}; - - std::shared_ptr table = - nt::NetworkTableInstance::GetDefault().GetTable("AprilTags"); - - frc846::ntinf::Pref can_bus_latency_{ - *this, "can_bus_latency", 20_ms}; - - // SPEAKER = Blue 7, Red 4 - std::map tag_locations{ - {4, {217.5_in, -1.5_in, 0_deg, 57.13_in}}, - {-3, {196.17_in, -1.5_in, 0_deg, 57.13_in}}, - {7, {217.5_in, 652.73_in, 180_deg, 57.13_in}}, - {-8, {196.17_in, 652.73_in, 180_deg, 57.13_in}}}; - - frc846::ntinf::Pref camera_height_{*this, "camera_height", - 14_in}; - frc846::ntinf::Pref camera_angle_{*this, "camera_angle", - 37_deg}; - frc846::ntinf::Pref camera_y_offset_{*this, "camera_y_offset", - -10_in}; - frc846::ntinf::Pref camera_x_offset_{*this, "camera_x_offset", - -7_in}; - - units::inch_t x_correction = 0_in; - units::inch_t y_correction = 0_in; + // frc846::ntinf::Pref default_is_red_side_{*this, + // "default_is_red_side"}; + + // frc846::ntinf::Grapher tag_in_sight_graph_{*this, "tag_in_sight"}; + // frc846::ntinf::Grapher tag_id_graph_{*this, "tag_id"}; + // frc846::ntinf::Grapher ll_latency_graph_{*this, + // "ll_latency"}; + + // frc846::base::Loggable readings_named{*this, "readings"}; + // frc846::ntinf::Grapher robot_x_graph_{readings_named, + // "robot_x"}; + // frc846::ntinf::Grapher robot_y_graph_{readings_named, + // "robot_y"}; + // frc846::ntinf::Grapher + // speaker_x_dist_graph_{readings_named, + // "speaker_dist_x"}; + // frc846::ntinf::Grapher + // speaker_y_dist_graph_{readings_named, + // "speaker_dist_y"}; + // frc846::ntinf::Grapher speaker_dist_graph_{readings_named, + // "speaker_dist"}; + // frc846::ntinf::Grapher local_x_dist_graph_{readings_named, + // "local_dist_x"}; + // frc846::ntinf::Grapher local_y_dist_graph_{readings_named, + // "local_dist_y"}; + // frc846::ntinf::Grapher tag_distance_graph_{readings_named, + // "tag_distance"}; + // frc846::ntinf::Grapher tag_angle_difference_graph_{ + // readings_named, "tag_angle_difference"}; + + // frc846::ntinf::Grapher velocity_in_component_graph_{ + // readings_named, "velocity_in_component"}; + // frc846::ntinf::Grapher velocity_orth_component_graph_{ + // readings_named, "velocity_orth_component"}; + + // std::shared_ptr table = + // nt::NetworkTableInstance::GetDefault().GetTable("AprilTags"); + + // frc846::ntinf::Pref can_bus_latency_{ + // *this, "can_bus_latency", 20_ms}; + + // // SPEAKER = Blue 7, Red 4 + // std::map tag_locations{ + // {4, {217.5_in, -1.5_in, 0_deg, 57.13_in}}, + // {-3, {196.17_in, -1.5_in, 0_deg, 57.13_in}}, + // {7, {217.5_in, 652.73_in, 180_deg, 57.13_in}}, + // {-8, {196.17_in, 652.73_in, 180_deg, 57.13_in}}}; + + // frc846::ntinf::Pref camera_height_{*this, "camera_height", + // 14_in}; + // frc846::ntinf::Pref camera_angle_{*this, "camera_angle", + // 37_deg}; + // frc846::ntinf::Pref camera_y_offset_{*this, + // "camera_y_offset", + // -10_in}; + // frc846::ntinf::Pref camera_x_offset_{*this, + // "camera_x_offset", + // -7_in}; + + // units::inch_t x_correction = 0_in; + // units::inch_t y_correction = 0_in; VisionReadings ReadFromHardware() override; diff --git a/src/y2024/include/subsystems/hardware/drivetrain.h b/src/y2024/include/subsystems/hardware/drivetrain.h index a94d209..10438cc 100644 --- a/src/y2024/include/subsystems/hardware/drivetrain.h +++ b/src/y2024/include/subsystems/hardware/drivetrain.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -25,6 +26,7 @@ struct DrivetrainReadings { frc846::math::FieldPoint pose; units::degrees_per_second_t angular_velocity; units::degree_t tilt; + frc846::math::FieldPoint april_pose; }; // Robot vs field oriented translation control. @@ -173,6 +175,31 @@ class DrivetrainSubsystem bool VerifyHardware() override; private: + // April Tags + std::map april_locations{ + {4, {217.5_in, -1.5_in, 0_deg, 57.13_in}}, + {3, {196.17_in, -1.5_in, 0_deg, 57.13_in}}, + {7, {217.5_in, 652.73_in, 180_deg, 57.13_in}}, + {8, {196.17_in, 652.73_in, 180_deg, 57.13_in}}}; + AprilTagCalculator april_calc{}; + + DrivetrainReadings trackTags(DrivetrainReadings input); + + frc846::base::Loggable april_named{*this, "april_tags"}; + frc846::ntinf::Pref camera_y_offset_{ + april_named, "camera_y_offset", -10_in}; // TODO: check + frc846::ntinf::Pref camera_x_offset_{april_named, + "camera_x_offset", -7_in}; + frc846::ntinf::Pref cam_angle_offset_{ + april_named, "camera_angle_offset", 180_deg}; + frc846::ntinf::Pref fudge_latency_{ + *this, "fudge_latency", 20_ms}; + frc846::ntinf::Grapher april_x_graph_{april_named, "april_x"}; + frc846::ntinf::Grapher april_y_graph_{april_named, "april_y"}; + + std::shared_ptr april_table = + nt::NetworkTableInstance::GetDefault().GetTable("AprilTags"); + units::feet_per_second_t vel_readings_composite; double vel_readings_composite_x; double vel_readings_composite_y;