From 38acc9a402be3d6900214229f4fe3aef63726cd6 Mon Sep 17 00:00:00 2001 From: Ari Date: Fri, 24 Apr 2026 16:03:34 +0000 Subject: [PATCH 01/25] Derive velocities from spline instead of using hypotenuse for smoother splines Signed-off-by: Ari --- src/pathing/controller.cc | 61 ++++++++++---------------- src/pathing/pathfinding.h | 3 +- src/pathing/splines.cc | 90 +++++++++++++++++++++++++++++---------- src/pathing/splines.h | 24 +++++++++-- 4 files changed, 113 insertions(+), 65 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 22e86f39..bd47b57d 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -1,24 +1,19 @@ #include #include -#include #include -#include #include #include -#include -#include -#include #include "splines.h" #include "src/localization/position_receiver.h" #include "src/pathing/splines.h" #include "src/utils/log.h" +#include "src/utils/pch.h" namespace pathing { auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { - const int lookahead_offset_ = 50; const double speed_ = 1.0; std::ifstream file(navgrid_path); @@ -56,14 +51,14 @@ auto RunController( auto next_pose_sub = inst.GetStructTopic("/pathing/nextPose").Publish(); - std::vector spline_points; + SplineResult result; while (true) { if (!enabled_sub.Get()) { vx_pub.Set(0.0); vy_pub.Set(0.0); - spline_points.clear(); + result.points.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } @@ -90,25 +85,24 @@ auto RunController( if (current_pose.Translation().Distance(t2d).value() < nodeSizeMeters) { vx_pub.Set(0.0); vy_pub.Set(0.0); - spline_points.clear(); + result.points.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } - if (spline_points.empty()) { - std::vector new_spline = - createSpline(grid, start_pt, target_pt, nodeSizeMeters); - if (!new_spline.empty()) { - spline_points = new_spline; + if (result.points.empty()) { + result = createSpline(grid, start_pt, target_pt, nodeSizeMeters); + if (!result.points.empty()) { + result.points = result.points; if (verbose) { - for (const auto& p : spline_points) { + for (const auto& p : result.points) { LOG(INFO) << "spline pt: " << p.X().value() << " " << p.Y().value(); } } } } - if (spline_points.empty()) { + if (result.points.empty()) { vx_pub.Set(0.0); vy_pub.Set(0.0); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -119,10 +113,10 @@ auto RunController( int closest_idx = 0; // TODO: this can be optimizeed by only searching from the previous closest index instead of the entire spline - frc::Translation2d first2d(spline_points[0].X(), spline_points[0].Y()); + frc::Translation2d first2d(result.points[0].X(), result.points[0].Y()); double best_dist = current_pose.Translation().Distance(first2d).value(); - for (int i = 1; i < (int)spline_points.size(); ++i) { - frc::Translation2d t2d(spline_points[i].X(), spline_points[i].Y()); + for (int i = 1; i < (int)result.points.size(); ++i) { + frc::Translation2d t2d(result.points[i].X(), result.points[i].Y()); double d = current_pose.Translation().Distance(t2d).value(); if (d < best_dist) { best_dist = d; @@ -135,42 +129,33 @@ auto RunController( if (verbose) { LOG(INFO) << "Closeset idx: " << closest_idx - << " Spline size: " << spline_points.size(); + << " Spline size: " << result.points.size(); } - int lookahead_idx = std::min(closest_idx + lookahead_offset_, - (int)spline_points.size() - 1); + std::pair derivative = + EvaluateDerivative(result.params[closest_idx], result.controls, + result.knots, result.p, 1); - frc::Pose2d lookahead = spline_points[lookahead_idx]; + auto [dx, dy] = derivative; if (verbose) { LOG(INFO) << "current " << current_pose.X().value() << " " << current_pose.Y().value(); } - double dx = lookahead.X().value() - current_pose.X().value(); - double dy = lookahead.Y().value() - current_pose.Y().value(); + double mag = std::hypot(dx, dy); if (verbose) { - LOG(INFO) << "dx " << dx << " vy " << dy; - LOG(INFO) << "looakhead " << lookahead.X().value() << " " - << lookahead.Y().value(); + LOG(INFO) << "dist " << mag; } - - next_pose_sub.Set(lookahead); - - double dist = std::hypot(dx, dy); - if (verbose) { - LOG(INFO) << "dist " << dist; - } - if (dist < 1e-6) { + if (mag < 1e-6) { vx_pub.Set(0.0); vy_pub.Set(0.0); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } - double vx = dx * speed_; - double vy = dy * speed_; + double vx = (dx / mag) * speed_; + double vy = (dy / mag) * speed_; // NOTE: we need to test whether to divide vx and vy by dist to normalize speeds or not, vx_pub.Set(vx); diff --git a/src/pathing/pathfinding.h b/src/pathing/pathfinding.h index 626051bf..200d649b 100644 --- a/src/pathing/pathfinding.h +++ b/src/pathing/pathfinding.h @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -9,7 +10,7 @@ namespace pathing { struct Node { uint x, y; - double cost = DBL_MAX; + double cost = INFINITY; bool visited = false; bool obstacle = false; char readable; diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index 24e457d6..b5840f8f 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -25,6 +25,33 @@ auto KnotVector(int n, int p) -> std::vector { return knots; } +auto FiniteDifferences(const std::vector>& controls, + const std::vector& knots, int p, int k) + -> std::vector> { + if (k == 0) { + return controls; + } + + std::vector> ret; + ret.reserve(controls.size() - 1); + + std::vector> prev = + FiniteDifferences(controls, knots, p, k - 1); + + for (int i = 0; i < (int)prev.size() - 1; ++i) { + double denom = knots[i + p + 1] - knots[i + k]; + if (denom != 0) { + double x = (prev[i + 1].first - prev[i].first) / denom; + double y = (prev[i + 1].second - prev[i].second) / denom; + ret.emplace_back(x, y); + } else { + ret.emplace_back(0.0, 0.0); + } + } + + return ret; +} + auto basis(int i, int p, double t, const std::vector& knots) -> double { if (p == 0) { if (knots[i] <= t && t < knots[i + 1]) { @@ -49,8 +76,9 @@ auto basis(int i, int p, double t, const std::vector& knots) -> double { return left + right; } -auto evaluate(double t, const std::vector>& controls, - const std::vector& knots, int p) +auto EvaluatePosition(double t, + const std::vector>& controls, + const std::vector& knots, int p) -> std::pair { if (t >= knots.back()) { return controls.back(); @@ -66,9 +94,24 @@ auto evaluate(double t, const std::vector>& controls, return {x, y}; } +auto EvaluateDerivative(double t, + const std::vector>& controls, + const std::vector& knots, int p, int k) + -> std::pair { + std::vector> first_diffs = + FiniteDifferences(controls, knots, p, k); + auto [x, y] = EvaluatePosition(t, first_diffs, knots, p - k); + + int n = 1; + for (int i = p; i >= p - k + 1; i--) { + n *= i; + } + return {x * n, y * n}; +} + auto createSpline(const std::vector>& grid, Point start_point, Point target_point, double nodeSizeMeters) - -> std::vector { + -> SplineResult { std::vector> gridCopy = grid; std::vector path = BFS(gridCopy, start_point, target_point); @@ -79,33 +122,36 @@ auto createSpline(const std::vector>& grid, } std::vector> control_points; + std::vector knots; + std::vector spline_points; + std::vector spline_params; + uint p; + control_points.reserve(path.size()); for (const pathing::Node& node : path) { control_points.emplace_back(node.x * nodeSizeMeters, node.y * nodeSizeMeters); - } + uint numControls = control_points.size(); - uint numControls = control_points.size(); - - if (numControls < 4) { - return {}; - } + if (numControls < 4) { + return {}; + } - uint p = 3; - if (numControls <= p) { - p = numControls - 1; - } + p = 3; + if (numControls <= p) { + p = numControls - 1; + } - std::vector knots = KnotVector(numControls, p); + std::vector knots = KnotVector(numControls, p); - std::vector spline_points; - for (int t = 0; t <= 1000; t += 1) { - double t_real = t / 1000.0; - auto [x, y] = evaluate(t_real, control_points, knots, p); - spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); + for (int t = 0; t <= 1000; t += 1) { + double t_real = t / 1000.0; + auto [x, y] = EvaluatePosition(t_real, control_points, knots, p); + spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); + spline_params.emplace_back(t_real); + } } - - return spline_points; -} + return {spline_points, control_points, knots, spline_params, p}; } // namespace pathing +} // namespace pathing \ No newline at end of file diff --git a/src/pathing/splines.h b/src/pathing/splines.h index f265c6b7..d5d28322 100644 --- a/src/pathing/splines.h +++ b/src/pathing/splines.h @@ -7,16 +7,32 @@ namespace pathing { +struct SplineResult { + std::vector points; + std::vector> controls; + std::vector knots; + std::vector params; + uint p; +}; + auto KnotVector(int n, int p) -> std::vector; +auto FiniteDifferences(const std::vector>& controls, + const std::vector& knots, int p, int k) + -> std::vector>; + auto basis(int i, int p, double t, const std::vector& knots) -> double; -auto evaluate(double t, const std::vector>& controls, - const std::vector& knots, int p) +auto EvaluatePosition(double t, + const std::vector>& controls, + const std::vector& knots, int p) + -> std::pair; +auto EvaluateDerivative(double t, + const std::vector>& controls, + const std::vector& knots, int p, int k) -> std::pair; auto createSpline(const std::vector>& grid, Point start_point, - Point target_point, double nodeSizeMeters) - -> std::vector; + Point target_point, double nodeSizeMeters) -> SplineResult; } // namespace pathing From 4c54eb96f5daac7d57adbee15137b7633139d265 Mon Sep 17 00:00:00 2001 From: Ari Date: Fri, 24 Apr 2026 16:11:53 +0000 Subject: [PATCH 02/25] Remove dead nt sub Signed-off-by: Ari --- src/pathing/controller.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index bd47b57d..e1b8062b 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -48,9 +48,6 @@ auto RunController( auto vx_pub = inst.GetDoubleTopic("/pathing/vx").Publish(); auto vy_pub = inst.GetDoubleTopic("/pathing/vy").Publish(); - auto next_pose_sub = - inst.GetStructTopic("/pathing/nextPose").Publish(); - SplineResult result; while (true) { From 276e08183883c3d12521f0a11976baa9ae1b0d36 Mon Sep 17 00:00:00 2001 From: Ari Date: Fri, 24 Apr 2026 16:40:11 +0000 Subject: [PATCH 03/25] Copilot comments Signed-off-by: Ari --- src/pathing/controller.cc | 2 -- src/pathing/splines.cc | 2 +- src/pathing/splines.h | 1 + 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index e1b8062b..e494bd32 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -5,7 +5,6 @@ #include #include "splines.h" #include "src/localization/position_receiver.h" -#include "src/pathing/splines.h" #include "src/utils/log.h" #include "src/utils/pch.h" @@ -90,7 +89,6 @@ auto RunController( if (result.points.empty()) { result = createSpline(grid, start_pt, target_pt, nodeSizeMeters); if (!result.points.empty()) { - result.points = result.points; if (verbose) { for (const auto& p : result.points) { LOG(INFO) << "spline pt: " << p.X().value() << " " << p.Y().value(); diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index b5840f8f..2fb7aad9 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -142,7 +142,7 @@ auto createSpline(const std::vector>& grid, p = numControls - 1; } - std::vector knots = KnotVector(numControls, p); + knots = KnotVector(numControls, p); for (int t = 0; t <= 1000; t += 1) { double t_real = t / 1000.0; diff --git a/src/pathing/splines.h b/src/pathing/splines.h index d5d28322..246f9d10 100644 --- a/src/pathing/splines.h +++ b/src/pathing/splines.h @@ -2,6 +2,7 @@ #include #include +#include #include #include "pathfinding.h" From ec34bc91134f075828d7cf93a6067077c3ba3296 Mon Sep 17 00:00:00 2001 From: Ari Date: Fri, 24 Apr 2026 16:49:27 +0000 Subject: [PATCH 04/25] More copilot comments Signed-off-by: Ari --- src/pathing/splines.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index 2fb7aad9..acd7f47c 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -32,6 +32,10 @@ auto FiniteDifferences(const std::vector>& controls, return controls; } + if (controls.size() <= 1 || k >= static_cast(controls.size())) { + return {}; + } + std::vector> ret; ret.reserve(controls.size() - 1); @@ -98,6 +102,10 @@ auto EvaluateDerivative(double t, const std::vector>& controls, const std::vector& knots, int p, int k) -> std::pair { + if (k < 0 || k > p || controls.size() <= static_cast(k)) { + return {0.0, 0.0}; + } + std::vector> first_diffs = FiniteDifferences(controls, knots, p, k); auto [x, y] = EvaluatePosition(t, first_diffs, knots, p - k); @@ -154,4 +162,4 @@ auto createSpline(const std::vector>& grid, return {spline_points, control_points, knots, spline_params, p}; } // namespace pathing -} // namespace pathing \ No newline at end of file +} // namespace pathing From 53f5a5d48bdc06f371e63a6374f2d8d1fe29afac Mon Sep 17 00:00:00 2001 From: Ari Date: Fri, 24 Apr 2026 17:52:28 +0000 Subject: [PATCH 05/25] More copilot comments Signed-off-by: Ari --- src/pathing/controller.cc | 12 ++++++------ src/pathing/splines.cc | 37 +++++++++++++++++++------------------ src/pathing/splines.h | 1 + 3 files changed, 26 insertions(+), 24 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index e494bd32..fdf1ff65 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -127,11 +127,12 @@ auto RunController( << " Spline size: " << result.points.size(); } - std::pair derivative = - EvaluateDerivative(result.params[closest_idx], result.controls, - result.knots, result.p, 1); - - auto [dx, dy] = derivative; + auto [dx_raw, dy_raw] = + EvaluatePosition(result.params[closest_idx], + result.first_deriv_controls, result.knots, + result.p - 1); + double dx = dx_raw * result.p; + double dy = dy_raw * result.p; if (verbose) { LOG(INFO) << "current " << current_pose.X().value() << " " @@ -152,7 +153,6 @@ auto RunController( double vx = (dx / mag) * speed_; double vy = (dy / mag) * speed_; - // NOTE: we need to test whether to divide vx and vy by dist to normalize speeds or not, vx_pub.Set(vx); vy_pub.Set(vy); if (verbose) { diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index acd7f47c..f585d4ac 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -139,27 +139,28 @@ auto createSpline(const std::vector>& grid, for (const pathing::Node& node : path) { control_points.emplace_back(node.x * nodeSizeMeters, node.y * nodeSizeMeters); - uint numControls = control_points.size(); + } - if (numControls < 4) { - return {}; - } + uint numControls = control_points.size(); + if (numControls < 4) { + return {}; + } - p = 3; - if (numControls <= p) { - p = numControls - 1; - } + p = 3; + if (numControls <= p) { + p = numControls - 1; + } - knots = KnotVector(numControls, p); + knots = KnotVector(numControls, p); - for (int t = 0; t <= 1000; t += 1) { - double t_real = t / 1000.0; - auto [x, y] = EvaluatePosition(t_real, control_points, knots, p); - spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); - spline_params.emplace_back(t_real); - } + for (int t = 0; t <= 1000; t += 1) { + double t_real = t / 1000.0; + auto [x, y] = EvaluatePosition(t_real, control_points, knots, p); + spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); + spline_params.emplace_back(t_real); } - return {spline_points, control_points, knots, spline_params, p}; - -} // namespace pathing + auto first_deriv_controls = FiniteDifferences(control_points, knots, p, 1); + return {spline_points, control_points, first_deriv_controls, + knots, spline_params, p}; +} } // namespace pathing diff --git a/src/pathing/splines.h b/src/pathing/splines.h index 246f9d10..d71cb213 100644 --- a/src/pathing/splines.h +++ b/src/pathing/splines.h @@ -11,6 +11,7 @@ namespace pathing { struct SplineResult { std::vector points; std::vector> controls; + std::vector> first_deriv_controls; std::vector knots; std::vector params; uint p; From 1dcd3b366a8662ea8dd1e7940c3b7654ccfb2215 Mon Sep 17 00:00:00 2001 From: Ari Date: Sun, 26 Apr 2026 19:13:32 +0000 Subject: [PATCH 06/25] final dp, robot was very very cooked Signed-off-by: Ari --- constants/navgrid.json | 330 +++++++++++----------- src/camera/cv_camera.cc | 9 +- src/localization/unambiguous_estimator.cc | 2 +- src/pathing/CMakeLists.txt | 3 + src/pathing/controller.cc | 13 +- src/pathing/simulator.cc | 188 +++++------- 6 files changed, 257 insertions(+), 288 deletions(-) diff --git a/constants/navgrid.json b/constants/navgrid.json index 0c0475ea..f83cd954 100644 --- a/constants/navgrid.json +++ b/constants/navgrid.json @@ -6,6 +6,7 @@ "nodeSizeMeters": 0.3, "grid": [ [ + true, true, false, false, @@ -15,19 +16,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -40,18 +28,30 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -64,6 +64,7 @@ true ], [ + true, true, false, false, @@ -73,6 +74,8 @@ false, false, false, + false, + false, true, true, true, @@ -84,9 +87,6 @@ true, true, true, - true, - true, - false, false, false, false, @@ -97,9 +97,6 @@ false, false, false, - false, - true, - true, true, true, true, @@ -118,10 +115,14 @@ false, false, false, + false, + false, + false, true, true ], [ + true, true, false, false, @@ -133,6 +134,17 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -143,28 +155,16 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -180,6 +180,7 @@ true ], [ + true, true, false, false, @@ -189,6 +190,8 @@ false, false, false, + false, + false, true, true, true, @@ -200,11 +203,6 @@ true, true, true, - true, - false, - false, - false, - false, false, false, false, @@ -225,7 +223,9 @@ true, true, true, - true, + false, + false, + false, false, false, false, @@ -238,6 +238,7 @@ true ], [ + true, true, false, false, @@ -248,8 +249,7 @@ false, false, false, - true, - true, + false, true, true, true, @@ -274,15 +274,15 @@ false, false, false, + false, true, true, true, true, true, true, - true, - true, - true, + false, + false, false, false, false, @@ -296,6 +296,7 @@ true ], [ + true, true, false, false, @@ -306,9 +307,7 @@ false, false, false, - true, - true, - true, + false, true, true, true, @@ -333,14 +332,15 @@ false, false, false, + false, true, true, true, true, true, true, - true, - true, + false, + false, false, false, false, @@ -354,6 +354,7 @@ true ], [ + true, true, false, false, @@ -372,10 +373,6 @@ true, true, true, - true, - true, - false, - false, false, false, false, @@ -391,13 +388,16 @@ false, false, false, - true, - false, false, false, false, true, true, + true, + true, + true, + true, + false, false, false, false, @@ -412,6 +412,7 @@ true ], [ + true, true, false, false, @@ -430,10 +431,6 @@ true, true, true, - true, - true, - false, - false, false, false, false, @@ -449,13 +446,16 @@ false, false, false, - true, - false, false, false, false, true, true, + true, + true, + true, + true, + false, false, false, false, @@ -470,6 +470,7 @@ true ], [ + true, true, false, false, @@ -488,8 +489,6 @@ true, true, true, - true, - true, false, false, false, @@ -507,13 +506,14 @@ false, false, false, - true, - false, - false, - false, false, true, true, + true, + true, + true, + true, + false, false, false, false, @@ -539,8 +539,7 @@ false, false, false, - true, - true, + false, true, true, true, @@ -565,7 +564,7 @@ false, false, false, - true, + false, true, true, true, @@ -581,6 +580,7 @@ false, false, false, + false, true, true, true @@ -1003,7 +1003,7 @@ false, false, false, - true, + false, true, true, true, @@ -1035,7 +1035,7 @@ true, true, true, - true, + false, false, false, false, @@ -1061,7 +1061,7 @@ false, false, false, - true, + false, true, true, true, @@ -1093,7 +1093,7 @@ true, true, true, - true, + false, false, false, false, @@ -1119,7 +1119,7 @@ false, false, false, - true, + false, true, true, true, @@ -1151,7 +1151,7 @@ true, true, true, - true, + false, false, false, false, @@ -1177,7 +1177,7 @@ false, false, false, - true, + false, true, true, true, @@ -1209,7 +1209,7 @@ true, true, true, - true, + false, false, false, false, @@ -1235,7 +1235,7 @@ false, false, false, - true, + false, true, true, true, @@ -1267,8 +1267,8 @@ true, true, true, - true, - true, + false, + false, false, false, false, @@ -1292,9 +1292,8 @@ false, false, false, - true, - true, - true, + false, + false, true, true, true, @@ -1318,15 +1317,16 @@ false, false, false, + false, + false, true, true, true, true, true, true, - true, - true, - true, + false, + false, false, false, false, @@ -1340,6 +1340,7 @@ true ], [ + true, true, false, false, @@ -1349,7 +1350,8 @@ false, false, false, - true, + false, + false, true, true, true, @@ -1371,10 +1373,6 @@ false, false, false, - false, - false, - false, - false, true, true, true, @@ -1394,10 +1392,13 @@ false, false, false, + false, + false, true, true ], [ + true, true, false, false, @@ -1409,6 +1410,17 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1419,28 +1431,16 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1456,6 +1456,7 @@ true ], [ + true, true, false, false, @@ -1465,6 +1466,8 @@ false, false, false, + false, + false, true, true, true, @@ -1476,9 +1479,6 @@ true, true, true, - true, - true, - false, false, false, false, @@ -1489,9 +1489,6 @@ false, false, false, - false, - true, - true, true, true, true, @@ -1510,10 +1507,14 @@ false, false, false, + false, + false, + false, true, true ], [ + true, true, false, false, @@ -1523,19 +1524,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -1548,18 +1536,30 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, diff --git a/src/camera/cv_camera.cc b/src/camera/cv_camera.cc index 96b6227f..3e858203 100644 --- a/src/camera/cv_camera.cc +++ b/src/camera/cv_camera.cc @@ -33,7 +33,13 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional log_path) LOG(INFO) << c.pipeline.value(); backup_image_ = cv::imread("/bos/constants/dont_worry_about_it.jpg"); - if (c.frame_height.has_value() && c.frame_width.has_value()) { + if (backup_image_.empty()) { + LOG(WARNING) << "Backup image /bos/constants/dont_worry_about_it.jpg not " + "found; using black placeholder"; + int w = c.frame_width.value_or(640); + int h = c.frame_height.value_or(480); + backup_image_ = cv::Mat::zeros(h, w, CV_8UC3); + } else if (c.frame_height.has_value() && c.frame_width.has_value()) { cv::resize(backup_image_, backup_image_, cv::Size(c.frame_width.value(), c.frame_height.value())); } @@ -77,6 +83,7 @@ auto CVCamera::GetFrame() -> timestamped_frame_t { if (timestamped_frame.frame.empty()) { timestamped_frame.frame = backup_image_; + LOG(INFO) << "Backup image used"; } if (timestamped_frame.frame.channels() == 4) { cv::cvtColor(timestamped_frame.frame, timestamped_frame.frame, diff --git a/src/localization/unambiguous_estimator.cc b/src/localization/unambiguous_estimator.cc index 8513b64b..1249cfb1 100644 --- a/src/localization/unambiguous_estimator.cc +++ b/src/localization/unambiguous_estimator.cc @@ -317,7 +317,7 @@ auto UnambiguousEstimator::GetUsableFrames( if (streamers_[i].has_value()) { streamers_[i].value().WriteFrame(frames[i].frame); } - if (!frames[i].invalid && + if (!frames[i].invalid && !frames[i].frame.empty() && latest_timestamp - frames[i].timestamp < kacceptable_frame_recency) { usable_frames[i] = std::move(frames[i]); } diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index 5d057b96..c46eecb5 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -1,2 +1,5 @@ add_library(pathing pathfinding.cc splines.cc controller.cc) target_link_libraries(pathing PUBLIC wpilibc wpiutil ntcore nlohmann_json::nlohmann_json utils localization ${OpenCV_LIBS}) + +add_executable(pathing_simulator simulator.cc) +target_link_libraries(pathing_simulator PRIVATE pathing) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index fdf1ff65..a29a0aff 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -13,6 +13,7 @@ namespace pathing { auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { + const uint lookahead_ = 5; const double speed_ = 1.0; std::ifstream file(navgrid_path); @@ -127,10 +128,10 @@ auto RunController( << " Spline size: " << result.points.size(); } - auto [dx_raw, dy_raw] = - EvaluatePosition(result.params[closest_idx], - result.first_deriv_controls, result.knots, - result.p - 1); + closest_idx += lookahead_; + auto [dx_raw, dy_raw] = EvaluatePosition(result.params[closest_idx], + result.first_deriv_controls, + result.knots, result.p - 1); double dx = dx_raw * result.p; double dy = dy_raw * result.p; @@ -150,8 +151,8 @@ auto RunController( continue; } - double vx = (dx / mag) * speed_; - double vy = (dy / mag) * speed_; + double vx = (dx / mag) * speed_ * nodeSizeMeters; + double vy = (dy / mag) * speed_ * nodeSizeMeters; vx_pub.Set(vx); vy_pub.Set(vy); diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 45b391e0..6bbac90c 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -1,149 +1,107 @@ #include -#include -#include +#include +#include +#include #include -#include +#include #include #include -#include #include -#include "src/pathing/pathing.h" +#include "pathfinding.h" +#include "splines.h" auto main() -> int { - wpi::log::DataLogBackgroundWriter log{"/root/bos/logs", "sim.wpilog"}; + const std::string navgrid_path = "/root/bos/constants/navgrid.json"; + const frc::Pose2d start{1_m, 1_m, frc::Rotation2d{}}; + const frc::Pose2d target{14_m, 6_m, frc::Rotation2d{}}; - wpi::log::StructLogEntry poseLog(log, "/sim/Pose2d"); - wpi::log::DoubleLogEntry accelXLog(log, "/sim/AccelX"); - wpi::log::DoubleLogEntry accelYLog(log, "/sim/AccelY"); - wpi::log::DoubleLogEntry accelMagLog(log, "/sim/AccelMagnitude"); - wpi::log::DoubleLogEntry velXLog(log, "/sim/VelX"); - wpi::log::DoubleLogEntry velYLog(log, "/sim/VelY"); - - std::ifstream file("/root/bos/constants/navgrid.json"); - if (!file.is_open()) { - return 1; - } + constexpr double kDt = 0.02; + constexpr double kSpeed = 1.0; + constexpr int kLookahead = 5; + constexpr int kMaxTicks = 30 * 50; + std::ifstream file(navgrid_path); nlohmann::json data = nlohmann::json::parse(file); file.close(); - const int GRID_W = data["grid"][0].size(); const int GRID_H = data["grid"].size(); - double nodeSizeMeters = data["nodeSizeMeters"]; + const int GRID_W = data["grid"][0].size(); + const double nodeSize = data["nodeSizeMeters"]; - std::vector> gridData(GRID_H, std::vector(GRID_W)); + std::vector> grid( + GRID_H, std::vector(GRID_W)); for (int y = 0; y < GRID_H; ++y) { for (int x = 0; x < GRID_W; ++x) { - gridData[y][x] = data["grid"][y][x]; + grid[y][x].x = x; + grid[y][x].y = y; + grid[y][x].obstacle = data["grid"][y][x]; } } - cv::Mat grid = initializeGrid(gridData); + pathing::Point start_pt{ + static_cast(start.X().value() / nodeSize), + static_cast(start.Y().value() / nodeSize)}; + pathing::Point target_pt{ + static_cast(target.X().value() / nodeSize), + static_cast(target.Y().value() / nodeSize)}; - auto poses = createSpline(grid, 10, 5, 45, 22, nodeSizeMeters); - if (poses.empty()) { + pathing::SplineResult spline = + pathing::createSpline(grid, start_pt, target_pt, nodeSize); + if (spline.points.empty()) { return 1; } - constexpr int64_t kDtUs = 20'000; - constexpr double kDtSec = kDtUs / 1'000'000.0; - constexpr double kMaxAccel = 3.0; - constexpr double kMaxDecel = 3.0; - constexpr double kMaxModuleSpeed = 5.0; - int64_t t = 0; - - std::vector pathDist(poses.size(), 0.0); - for (size_t i = 1; i < poses.size(); ++i) { - double dx = poses[i].X().value() - poses[i - 1].X().value(); - double dy = poses[i].Y().value() - poses[i - 1].Y().value(); - pathDist[i] = pathDist[i - 1] + std::sqrt(dx * dx + dy * dy); - } - - double totalDist = pathDist.back(); - - std::vector targetSpeed(poses.size()); - - for (size_t i = 0; i < poses.size(); ++i) { - double distFromStart = pathDist[i]; - double distToEnd = totalDist - pathDist[i]; - - double accelLimitedSpeed = std::sqrt(2.0 * kMaxAccel * distFromStart); - double decelLimitedSpeed = std::sqrt(2.0 * kMaxDecel * distToEnd); - - double maxVelThisPose = kMaxModuleSpeed; - if (i > 0) { - double dx = poses[i].X().value() - poses[i - 1].X().value(); - double dy = poses[i].Y().value() - poses[i - 1].Y().value(); - double vx = dx / kDtSec; - double vy = dy / kDtSec; - double maxComponentSpeed = std::max(std::abs(vx), std::abs(vy)); - if (maxComponentSpeed > 0.001) { - double speedRatio = std::sqrt(vx * vx + vy * vy) / maxComponentSpeed; - maxVelThisPose = kMaxModuleSpeed / speedRatio; - } - } - - targetSpeed[i] = - std::min({accelLimitedSpeed, decelLimitedSpeed, maxVelThisPose}); - } - - double currentVx = 0.0; - double currentVy = 0.0; - double currentX = poses[0].X().value(); - double currentY = poses[0].Y().value(); - double currentSpeed = 0.0; - - for (size_t i = 0; i < poses.size(); ++i) { - frc::Pose2d actualPose{units::meter_t{currentX}, units::meter_t{currentY}, - poses[i].Rotation()}; - poseLog.Append(actualPose, t); + std::remove("/root/bos/logs/sim.wpilog"); + std::error_code ec; + wpi::log::DataLogWriter log{"/root/bos/logs/sim.wpilog", ec}; + if (ec) return 1; + wpi::log::StructLogEntry pose_log(log, "/sim/Pose"); + wpi::log::DoubleLogEntry vx_log(log, "/sim/Vx"); + wpi::log::DoubleLogEntry vy_log(log, "/sim/Vy"); - double desiredSpeed = targetSpeed[i]; + double x = start.X().value(); + double y = start.Y().value(); + int64_t t_us = 0; - double dvMag = desiredSpeed - currentSpeed; - double accelMag = 0.0; + for (int tick = 0; tick < kMaxTicks; ++tick) { + pose_log.Append({units::meter_t{x}, units::meter_t{y}, frc::Rotation2d{}}, + t_us); - if (dvMag > 0) { - accelMag = std::min(dvMag / kDtSec, kMaxAccel); - } else { - accelMag = std::max(dvMag / kDtSec, -kMaxDecel); + if (std::hypot(target.X().value() - x, target.Y().value() - y) < nodeSize) { + break; } - currentSpeed += accelMag * kDtSec; - currentSpeed = std::max(0.0, currentSpeed); - - if (i > 0) { - double dx = poses[i].X().value() - poses[i - 1].X().value(); - double dy = poses[i].Y().value() - poses[i - 1].Y().value(); - double segDist = std::sqrt(dx * dx + dy * dy); - - double dirX = segDist > 0.001 ? dx / segDist : 0.0; - double dirY = segDist > 0.001 ? dy / segDist : 0.0; - - double newVx = dirX * currentSpeed; - double newVy = dirY * currentSpeed; - - double ax = (newVx - currentVx) / kDtSec; - double ay = (newVy - currentVy) / kDtSec; - - currentVx = newVx; - currentVy = newVy; - - currentX += currentVx * kDtSec; - currentY += currentVy * kDtSec; - - double accelMagTotal = std::sqrt(ax * ax + ay * ay); - - accelXLog.Append(ax, t); - accelYLog.Append(ay, t); - accelMagLog.Append(accelMagTotal, t); - velXLog.Append(currentVx, t); - velYLog.Append(currentVy, t); + int closest = 0; + double best = std::hypot(spline.points[0].X().value() - x, + spline.points[0].Y().value() - y); + for (int i = 1; i < (int)spline.points.size(); ++i) { + double d = std::hypot(spline.points[i].X().value() - x, + spline.points[i].Y().value() - y); + if (d < best) { + best = d; + closest = i; + } } - t += kDtUs; + int idx = std::min(closest + kLookahead, (int)spline.points.size() - 1); + auto [dx_raw, dy_raw] = pathing::EvaluatePosition( + spline.params[idx], spline.first_deriv_controls, spline.knots, + spline.p - 1); + double dx = dx_raw * spline.p; + double dy = dy_raw * spline.p; + double mag = std::hypot(dx, dy); + if (mag < 1e-6) break; + + double vx = (dx / mag) * kSpeed; + double vy = (dy / mag) * kSpeed; + vx_log.Append(vx, t_us); + vy_log.Append(vy, t_us); + + x += vx * kDt; + y += vy * kDt; + t_us += static_cast(kDt * 1'000'000); } log.Flush(); return 0; -} \ No newline at end of file +} From 0c8106256087e7952228ebbc4371891e28800cad Mon Sep 17 00:00:00 2001 From: Ari Date: Sun, 3 May 2026 17:07:13 +0000 Subject: [PATCH 07/25] get into kinda working state with sim, will debug more. * simulator is just a copy of controller with added simulation at the end Signed-off-by: Ari --- constants/navgrid.json | 586 +++++++++++++++++++-------------------- src/pathing/simulator.cc | 164 +++++------ src/pathing/splines.cc | 3 +- 3 files changed, 379 insertions(+), 374 deletions(-) diff --git a/constants/navgrid.json b/constants/navgrid.json index f83cd954..8969d094 100644 --- a/constants/navgrid.json +++ b/constants/navgrid.json @@ -8,74 +8,9 @@ [ true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, - true - ], - [ true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -87,16 +22,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -107,33 +32,40 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, true, true ], [ true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -145,16 +77,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, true, @@ -163,6 +85,43 @@ true, true, true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ true, true, false, @@ -176,12 +135,6 @@ false, false, false, - true, - true - ], - [ - true, - true, false, false, false, @@ -192,17 +145,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -213,16 +155,16 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -291,7 +233,7 @@ false, false, false, - true, + false, true, true ], @@ -348,7 +290,7 @@ false, false, false, - true, + false, true, true, true @@ -424,13 +366,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -449,12 +384,19 @@ false, false, false, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -482,13 +424,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -507,12 +442,19 @@ false, false, false, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -528,9 +470,6 @@ true ], [ - true, - true, - true, true, true, false, @@ -540,13 +479,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -565,12 +497,21 @@ false, false, false, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -583,6 +524,7 @@ false, true, true, + true, true ], [ @@ -591,14 +533,13 @@ true, true, true, - true, false, false, false, false, false, - true, - true, + false, + false, true, true, true, @@ -622,8 +563,8 @@ false, false, false, - true, - true, + false, + false, true, true, true, @@ -641,6 +582,7 @@ false, false, true, + true, true ], [ @@ -695,9 +637,9 @@ false, false, false, - true, - true, - true, + false, + false, + false, true, true ], @@ -752,7 +694,7 @@ false, false, false, - true, + false, true, true, true, @@ -881,7 +823,7 @@ true, true, true, - false, + true, false, false, false, @@ -936,9 +878,9 @@ [ true, true, - false, - false, - false, + true, + true, + true, false, false, false, @@ -1003,7 +945,6 @@ false, false, false, - false, true, true, true, @@ -1011,8 +952,8 @@ true, true, true, - false, - false, + true, + true, false, false, false, @@ -1035,14 +976,15 @@ true, true, true, + true, + true, false, false, false, false, false, false, - false, - false, + true, true, true, true, @@ -1052,7 +994,7 @@ [ true, true, - true, + false, false, false, false, @@ -1101,9 +1043,9 @@ false, false, false, - false, - false, - false, + true, + true, + true, true, true ], @@ -1120,14 +1062,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, - false, false, false, false, @@ -1145,12 +1079,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, false, false, false, @@ -1162,13 +1090,6 @@ false, false, false, - true, - true - ], - [ - true, - true, - true, false, false, false, @@ -1178,18 +1099,18 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, false, false, + true, + true + ], + [ + true, + true, + true, false, false, false, @@ -1203,12 +1124,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, false, false, false, @@ -1220,13 +1135,6 @@ false, false, false, - true, - true - ], - [ - true, - true, - true, false, false, false, @@ -1236,13 +1144,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -1262,8 +1163,9 @@ false, false, true, - true, - true, + true + ], + [ true, true, true, @@ -1278,12 +1180,6 @@ false, false, false, - true, - true - ], - [ - true, - true, false, false, false, @@ -1294,13 +1190,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -1319,12 +1208,7 @@ false, false, false, - true, - true, - true, - true, - true, - true, + false, false, false, false, @@ -1340,6 +1224,7 @@ true ], [ + true, true, true, false, @@ -1351,11 +1236,6 @@ false, false, false, - false, - true, - true, - true, - true, true, true, true, @@ -1373,10 +1253,14 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, true, true, true, @@ -1417,10 +1301,6 @@ true, true, true, - true, - true, - true, - true, false, false, false, @@ -1431,10 +1311,14 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, true, true, true, @@ -1475,10 +1359,6 @@ true, true, true, - true, - true, - true, - true, false, false, false, @@ -1489,10 +1369,14 @@ false, false, false, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, true, true, true, @@ -1570,6 +1454,122 @@ false, true, true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true ] ] } \ No newline at end of file diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 6bbac90c..bfe5df55 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -1,105 +1,109 @@ #include -#include +#include #include -#include +#include +#include #include -#include +#include #include #include +#include +#include #include -#include "pathfinding.h" -#include "splines.h" +#include "src/pathing/splines.h" auto main() -> int { - const std::string navgrid_path = "/root/bos/constants/navgrid.json"; - const frc::Pose2d start{1_m, 1_m, frc::Rotation2d{}}; - const frc::Pose2d target{14_m, 6_m, frc::Rotation2d{}}; + const uint lookahead_ = 5; + constexpr int64_t k_dt_us = 20'000; + constexpr double k_dt_sec = 0.02; - constexpr double kDt = 0.02; - constexpr double kSpeed = 1.0; - constexpr int kLookahead = 5; - constexpr int kMaxTicks = 30 * 50; - - std::ifstream file(navgrid_path); + std::ifstream file("/bos/constants/navgrid.json"); + if (!file.is_open()) { + return 1; + } nlohmann::json data = nlohmann::json::parse(file); file.close(); - const int GRID_H = data["grid"].size(); - const int GRID_W = data["grid"][0].size(); - const double nodeSize = data["nodeSizeMeters"]; + const int grid_h = data["grid"].size(); + const int grid_w = data["grid"][0].size(); + const double node_size_meters = data["nodeSizeMeters"]; std::vector> grid( - GRID_H, std::vector(GRID_W)); - for (int y = 0; y < GRID_H; ++y) { - for (int x = 0; x < GRID_W; ++x) { + grid_h, std::vector(grid_w)); + for (int y = 0; y < grid_h; ++y) { + for (int x = 0; x < grid_w; ++x) { grid[y][x].x = x; grid[y][x].y = y; grid[y][x].obstacle = data["grid"][y][x]; } } - pathing::Point start_pt{ - static_cast(start.X().value() / nodeSize), - static_cast(start.Y().value() / nodeSize)}; - pathing::Point target_pt{ - static_cast(target.X().value() / nodeSize), - static_cast(target.Y().value() / nodeSize)}; - - pathing::SplineResult spline = - pathing::createSpline(grid, start_pt, target_pt, nodeSize); - if (spline.points.empty()) { - return 1; - } - - std::remove("/root/bos/logs/sim.wpilog"); - std::error_code ec; - wpi::log::DataLogWriter log{"/root/bos/logs/sim.wpilog", ec}; - if (ec) return 1; - wpi::log::StructLogEntry pose_log(log, "/sim/Pose"); - wpi::log::DoubleLogEntry vx_log(log, "/sim/Vx"); - wpi::log::DoubleLogEntry vy_log(log, "/sim/Vy"); - - double x = start.X().value(); - double y = start.Y().value(); - int64_t t_us = 0; - - for (int tick = 0; tick < kMaxTicks; ++tick) { - pose_log.Append({units::meter_t{x}, units::meter_t{y}, frc::Rotation2d{}}, - t_us); - - if (std::hypot(target.X().value() - x, target.Y().value() - y) < nodeSize) { - break; - } - - int closest = 0; - double best = std::hypot(spline.points[0].X().value() - x, - spline.points[0].Y().value() - y); - for (int i = 1; i < (int)spline.points.size(); ++i) { - double d = std::hypot(spline.points[i].X().value() - x, - spline.points[i].Y().value() - y); - if (d < best) { - best = d; - closest = i; - } + int start_x = 10; + int start_y = 5; + int target_x = 22; + int target_y = 14; + start_x = std::clamp(start_x, 0, grid_w - 1); + start_y = std::clamp(start_y, 0, grid_h - 1); + target_x = std::clamp(target_x, 0, grid_w - 1); + target_y = std::clamp(target_y, 0, grid_h - 1); + + frc::Pose2d initial_pose(units::meter_t{start_x * node_size_meters}, + units::meter_t{start_y * node_size_meters}, + units::radian_t{0.0}); + const frc::Pose2d target_pose(units::meter_t{target_x * node_size_meters}, + units::meter_t{target_y * node_size_meters}, + units::radian_t{0.0}); + + std::vector> velocities; + pathing::Point start_pt{.x = static_cast(start_x), + .y = static_cast(start_y)}; + pathing::Point target_pt{.x = static_cast(target_x), + .y = static_cast(target_y)}; + pathing::SplineResult result = + pathing::createSpline(grid, start_pt, target_pt, node_size_meters); + if (result.points.empty()) { + velocities.emplace_back(0.0, 0.0); + } else { + velocities.reserve(result.params.size()); + for (int i = 0; i < static_cast(result.params.size()); ++i) { + const int idx = + std::min(i + static_cast(lookahead_), + static_cast(result.params.size()) - 1); + const auto [raw_vx, raw_vy] = + pathing::EvaluateDerivative(result.params[idx], result.controls, + result.knots, result.p, 1); + + const int prev_idx = std::max(idx - 1, 0); + const int next_idx = + std::min(idx + 1, static_cast(result.params.size()) - 1); + const double param_delta = + std::max(result.params[next_idx] - result.params[prev_idx], 1e-9); + const double sample_dt = (next_idx - prev_idx) * k_dt_sec; + const double scale = sample_dt > 0.0 ? (param_delta / sample_dt) : 0.0; + + velocities.emplace_back(raw_vx * scale, raw_vy * scale); } + } - int idx = std::min(closest + kLookahead, (int)spline.points.size() - 1); - auto [dx_raw, dy_raw] = pathing::EvaluatePosition( - spline.params[idx], spline.first_deriv_controls, spline.knots, - spline.p - 1); - double dx = dx_raw * spline.p; - double dy = dy_raw * spline.p; - double mag = std::hypot(dx, dy); - if (mag < 1e-6) break; - - double vx = (dx / mag) * kSpeed; - double vy = (dy / mag) * kSpeed; - vx_log.Append(vx, t_us); - vy_log.Append(vy, t_us); - - x += vx * kDt; - y += vy * kDt; - t_us += static_cast(kDt * 1'000'000); + wpi::log::DataLogBackgroundWriter log{"logs", "sim.wpilog"}; + log.AddStructSchema(); + wpi::log::StructLogEntry pose_log(log, "/sim/Pose2d"); + wpi::log::StructLogEntry target_log(log, "/sim/TargetPose2d"); + wpi::log::DoubleLogEntry vx_log(log, "/sim/pathing/vx"); + wpi::log::DoubleLogEntry vy_log(log, "/sim/pathing/vy"); + + target_log.Append(target_pose, 1); + frc::Pose2d replay_pose = initial_pose; + int64_t t = 0; + for (const auto& [vx, vy] : velocities) { + vx_log.Append(vx, t); + vy_log.Append(vy, t); + pose_log.Append(replay_pose, t); + replay_pose = + frc::Pose2d(units::meter_t{replay_pose.X().value() + vx * k_dt_sec}, + units::meter_t{replay_pose.Y().value() + vy * k_dt_sec}, + units::radian_t{0.0}); + t += k_dt_us; } log.Flush(); diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index f585d4ac..ee3f4b9d 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -156,7 +156,8 @@ auto createSpline(const std::vector>& grid, for (int t = 0; t <= 1000; t += 1) { double t_real = t / 1000.0; auto [x, y] = EvaluatePosition(t_real, control_points, knots, p); - spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); + spline_points.emplace_back(units::meter_t{x * nodeSizeMeters}, + units::meter_t{y * nodeSizeMeters}, 0_rad); spline_params.emplace_back(t_real); } auto first_deriv_controls = FiniteDifferences(control_points, knots, p, 1); From 7148d4fa1a0e53a7dddd4495dbc41dbf89f51470 Mon Sep 17 00:00:00 2001 From: Ari Date: Sun, 3 May 2026 17:53:12 +0000 Subject: [PATCH 08/25] update controller with the same changes that made simulator work Signed-off-by: Ari --- src/pathing/controller.cc | 27 +++------------------------ 1 file changed, 3 insertions(+), 24 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index a29a0aff..3e07513a 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -14,7 +14,6 @@ auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { const uint lookahead_ = 5; - const double speed_ = 1.0; std::ifstream file(navgrid_path); if (!file.is_open()) { @@ -129,30 +128,10 @@ auto RunController( } closest_idx += lookahead_; - auto [dx_raw, dy_raw] = EvaluatePosition(result.params[closest_idx], - result.first_deriv_controls, - result.knots, result.p - 1); - double dx = dx_raw * result.p; - double dy = dy_raw * result.p; - if (verbose) { - LOG(INFO) << "current " << current_pose.X().value() << " " - << current_pose.Y().value(); - } - - double mag = std::hypot(dx, dy); - if (verbose) { - LOG(INFO) << "dist " << mag; - } - if (mag < 1e-6) { - vx_pub.Set(0.0); - vy_pub.Set(0.0); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - continue; - } - - double vx = (dx / mag) * speed_ * nodeSizeMeters; - double vy = (dy / mag) * speed_ * nodeSizeMeters; + const auto [vx, vy] = + EvaluateDerivative(result.params[closest_idx], result.controls, + result.knots, result.p, 1); vx_pub.Set(vx); vy_pub.Set(vy); From c1c413cb03d349323833cc8e73c6ded85703f420 Mon Sep 17 00:00:00 2001 From: Ari Date: Mon, 4 May 2026 18:20:41 +0000 Subject: [PATCH 09/25] even more simulator Signed-off-by: Ari --- constants/navgrid.json | 198 ++++++++++++++++++------------------- src/pathing/pathfinding.cc | 47 +++++++++ src/pathing/simulator.cc | 108 +++++++++++--------- src/pathing/splines.cc | 5 +- 4 files changed, 212 insertions(+), 146 deletions(-) diff --git a/constants/navgrid.json b/constants/navgrid.json index 8969d094..ffb1e806 100644 --- a/constants/navgrid.json +++ b/constants/navgrid.json @@ -192,13 +192,13 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -366,6 +366,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -384,19 +391,12 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -424,6 +424,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -442,19 +449,12 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -482,13 +482,13 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -540,13 +540,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -565,12 +558,19 @@ false, false, false, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1004,13 +1004,6 @@ false, false, false, - true, - true, - true, - true, - true, - true, - true, false, false, false, @@ -1029,12 +1022,19 @@ false, false, false, - true, - true, - true, - true, - true, - true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, false, false, false, @@ -1062,13 +1062,13 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1120,6 +1120,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1138,19 +1145,12 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1178,6 +1178,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1196,19 +1203,12 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, diff --git a/src/pathing/pathfinding.cc b/src/pathing/pathfinding.cc index 8b0b2bf3..e862b54c 100644 --- a/src/pathing/pathfinding.cc +++ b/src/pathing/pathfinding.cc @@ -9,11 +9,55 @@ namespace pathing { +auto BFSFirstFreeCell(std::vector>& field, Point start_point) + -> Node { + int sx = start_point.x; + int sy = start_point.y; + + field[sy][sx].visited = true; + + std::deque queue; + queue.push_back(start_point); + + std::vector> dirs = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1}, + {0, 1}, {1, -1}, {1, 0}, {1, 1}}; + + while (!queue.empty()) { + const Point current_point = queue.front(); + queue.pop_front(); + + Node& current = field[current_point.y][current_point.x]; + + for (auto [dy, dx] : dirs) { + int nx = current.x + dx; + int ny = current.y + dy; + + if (nx >= 0 && nx < (int)field[0].size() && ny >= 0 && + ny < (int)field.size()) { + if (!field[ny][nx].visited) { + field[ny][nx].visited = true; + if (!field[ny][nx].obstacle) { + return field[ny][nx]; + } + queue.push_back({.x = (uint)nx, .y = (uint)ny}); + } + } + } + } + + return field[sy][sx]; +} + auto BFS(std::vector>& field, Point start_point, Point end_point) -> std::vector { int sx = start_point.x; int sy = start_point.y; + if (field[sy][sx].obstacle) { + Node adjusted = BFSFirstFreeCell(field, start_point); + sx = adjusted.x; + sy = adjusted.y; + } Node* start = &field[sy][sx]; start->visited = true; @@ -85,6 +129,9 @@ auto BFS(std::vector>& field, Point start_point, field[rcurrent->y][rcurrent->x].path = true; } } + if (rcurrent != nullptr) { + rpath.push_back(*rcurrent); + } std::reverse(rpath.begin(), rpath.end()); std::cout << "BFS path (" << start_point.x << "," << start_point.y << ") -> (" diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index bfe5df55..d63a54e2 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -7,15 +7,13 @@ #include #include #include -#include -#include #include #include "src/pathing/splines.h" auto main() -> int { const uint lookahead_ = 5; - constexpr int64_t k_dt_us = 20'000; - constexpr double k_dt_sec = 0.02; + const int64_t dt_us = 20'000; + const double dt_sec = 0.02; std::ifstream file("/bos/constants/navgrid.json"); if (!file.is_open()) { @@ -47,63 +45,85 @@ auto main() -> int { target_x = std::clamp(target_x, 0, grid_w - 1); target_y = std::clamp(target_y, 0, grid_h - 1); - frc::Pose2d initial_pose(units::meter_t{start_x * node_size_meters}, + frc::Pose2d current_pose(units::meter_t{start_x * node_size_meters}, units::meter_t{start_y * node_size_meters}, units::radian_t{0.0}); const frc::Pose2d target_pose(units::meter_t{target_x * node_size_meters}, units::meter_t{target_y * node_size_meters}, units::radian_t{0.0}); - std::vector> velocities; - pathing::Point start_pt{.x = static_cast(start_x), - .y = static_cast(start_y)}; - pathing::Point target_pt{.x = static_cast(target_x), - .y = static_cast(target_y)}; - pathing::SplineResult result = - pathing::createSpline(grid, start_pt, target_pt, node_size_meters); - if (result.points.empty()) { - velocities.emplace_back(0.0, 0.0); - } else { - velocities.reserve(result.params.size()); - for (int i = 0; i < static_cast(result.params.size()); ++i) { - const int idx = - std::min(i + static_cast(lookahead_), - static_cast(result.params.size()) - 1); - const auto [raw_vx, raw_vy] = - pathing::EvaluateDerivative(result.params[idx], result.controls, - result.knots, result.p, 1); - - const int prev_idx = std::max(idx - 1, 0); - const int next_idx = - std::min(idx + 1, static_cast(result.params.size()) - 1); - const double param_delta = - std::max(result.params[next_idx] - result.params[prev_idx], 1e-9); - const double sample_dt = (next_idx - prev_idx) * k_dt_sec; - const double scale = sample_dt > 0.0 ? (param_delta / sample_dt) : 0.0; - - velocities.emplace_back(raw_vx * scale, raw_vy * scale); - } - } - wpi::log::DataLogBackgroundWriter log{"logs", "sim.wpilog"}; log.AddStructSchema(); wpi::log::StructLogEntry pose_log(log, "/sim/Pose2d"); wpi::log::StructLogEntry target_log(log, "/sim/TargetPose2d"); wpi::log::DoubleLogEntry vx_log(log, "/sim/pathing/vx"); wpi::log::DoubleLogEntry vy_log(log, "/sim/pathing/vy"); - target_log.Append(target_pose, 1); - frc::Pose2d replay_pose = initial_pose; + + pathing::SplineResult result; + constexpr int k_max_steps = 10000; int64_t t = 0; - for (const auto& [vx, vy] : velocities) { + for (int step = 0; step < k_max_steps; ++step) { + pathing::Point start_pt{ + .x = static_cast(current_pose.X().value() / node_size_meters), + .y = static_cast(current_pose.Y().value() / node_size_meters)}; + pathing::Point target_pt{ + .x = static_cast(target_pose.X().value() / node_size_meters), + .y = static_cast(target_pose.Y().value() / node_size_meters)}; + start_pt.x = std::clamp(static_cast(start_pt.x), 0, grid_w - 1); + start_pt.y = std::clamp(static_cast(start_pt.y), 0, grid_h - 1); + target_pt.x = std::clamp(static_cast(target_pt.x), 0, grid_w - 1); + target_pt.y = std::clamp(static_cast(target_pt.y), 0, grid_h - 1); + + frc::Translation2d t2d(target_pose.X(), target_pose.Y()); + if (current_pose.Translation().Distance(t2d).value() < node_size_meters) { + vx_log.Append(0.0, t); + vy_log.Append(0.0, t); + pose_log.Append(current_pose, t); + break; + } + + if (result.points.empty()) { + result = + pathing::createSpline(grid, start_pt, target_pt, node_size_meters); + } + + if (result.points.empty()) { + vx_log.Append(0.0, t); + vy_log.Append(0.0, t); + pose_log.Append(current_pose, t); + break; + } + + int closest_idx = 0; + frc::Translation2d first2d(result.points[0].X(), result.points[0].Y()); + double best_dist = current_pose.Translation().Distance(first2d).value(); + for (int i = 1; i < static_cast(result.points.size()); ++i) { + frc::Translation2d p(result.points[i].X(), result.points[i].Y()); + double d = current_pose.Translation().Distance(p).value(); + if (d < best_dist) { + best_dist = d; + closest_idx = i; + } + } + + closest_idx += lookahead_; + if (closest_idx >= static_cast(result.params.size())) { + closest_idx = static_cast(result.params.size()) - 1; + } + + const auto [vx, vy] = pathing::EvaluateDerivative( + result.params[closest_idx], result.controls, result.knots, result.p, 1); vx_log.Append(vx, t); vy_log.Append(vy, t); - pose_log.Append(replay_pose, t); - replay_pose = - frc::Pose2d(units::meter_t{replay_pose.X().value() + vx * k_dt_sec}, - units::meter_t{replay_pose.Y().value() + vy * k_dt_sec}, + pose_log.Append(current_pose, t); + + current_pose = + frc::Pose2d(units::meter_t{current_pose.X().value() + vx * dt_sec}, + units::meter_t{current_pose.Y().value() + vy * dt_sec}, units::radian_t{0.0}); - t += k_dt_us; + result = {}; + t += dt_us; } log.Flush(); diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index ee3f4b9d..e612cf21 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -146,7 +146,7 @@ auto createSpline(const std::vector>& grid, return {}; } - p = 3; + p = 4; if (numControls <= p) { p = numControls - 1; } @@ -156,8 +156,7 @@ auto createSpline(const std::vector>& grid, for (int t = 0; t <= 1000; t += 1) { double t_real = t / 1000.0; auto [x, y] = EvaluatePosition(t_real, control_points, knots, p); - spline_points.emplace_back(units::meter_t{x * nodeSizeMeters}, - units::meter_t{y * nodeSizeMeters}, 0_rad); + spline_points.emplace_back(units::meter_t{x}, units::meter_t{y}, 0_rad); spline_params.emplace_back(t_real); } auto first_deriv_controls = FiniteDifferences(control_points, knots, p, 1); From 40e5c677759e5cfdc868f04af125b75775a2396d Mon Sep 17 00:00:00 2001 From: Ari Date: Mon, 4 May 2026 18:21:32 +0000 Subject: [PATCH 10/25] update controller Signed-off-by: Ari --- src/pathing/controller.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 3e07513a..c05a41f1 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -128,6 +128,9 @@ auto RunController( } closest_idx += lookahead_; + if (closest_idx >= static_cast(result.params.size())) { + closest_idx = static_cast(result.params.size()) - 1; + } const auto [vx, vy] = EvaluateDerivative(result.params[closest_idx], result.controls, @@ -139,6 +142,7 @@ auto RunController( LOG(INFO) << "set vx " << vx << " vy " << vy; } inst.Flush(); + result = {}; } } } From b04683bd7244c9d7724ac27b0343ed59cb7b3e0a Mon Sep 17 00:00:00 2001 From: Ari Date: Mon, 4 May 2026 20:52:39 +0000 Subject: [PATCH 11/25] Fix start point and remove replanning Signed-off-by: Ari --- src/pathing/pathfinding.cc | 2 ++ src/pathing/simulator.cc | 5 ++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/pathing/pathfinding.cc b/src/pathing/pathfinding.cc index e862b54c..7bb526cf 100644 --- a/src/pathing/pathfinding.cc +++ b/src/pathing/pathfinding.cc @@ -60,6 +60,8 @@ auto BFS(std::vector>& field, Point start_point, } Node* start = &field[sy][sx]; + start_point.x = sx; + start_point.y = sy; start->visited = true; start->cost = 0; diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index d63a54e2..51496475 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -38,8 +38,8 @@ auto main() -> int { int start_x = 10; int start_y = 5; - int target_x = 22; - int target_y = 14; + int target_x = 21; + int target_y = 12; start_x = std::clamp(start_x, 0, grid_w - 1); start_y = std::clamp(start_y, 0, grid_h - 1); target_x = std::clamp(target_x, 0, grid_w - 1); @@ -122,7 +122,6 @@ auto main() -> int { frc::Pose2d(units::meter_t{current_pose.X().value() + vx * dt_sec}, units::meter_t{current_pose.Y().value() + vy * dt_sec}, units::radian_t{0.0}); - result = {}; t += dt_us; } From cb834438fe62491156c1630a69b396fdedf6d9de Mon Sep 17 00:00:00 2001 From: Ari Date: Mon, 4 May 2026 23:56:27 +0000 Subject: [PATCH 12/25] Fix up simulator Signed-off-by: Ari --- src/pathing/simulator.cc | 35 ++++++++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 51496475..6d6a49ca 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -11,11 +11,12 @@ #include "src/pathing/splines.h" auto main() -> int { - const uint lookahead_ = 5; - const int64_t dt_us = 20'000; - const double dt_sec = 0.02; + const uint lookahead_ = 0; + const int64_t dt_us = 20000; + const double dt_sec = 0.020; + const double speed_ = 0.1; - std::ifstream file("/bos/constants/navgrid.json"); + std::ifstream file("/root/bos/constants/navgrid.json"); if (!file.is_open()) { return 1; } @@ -38,7 +39,7 @@ auto main() -> int { int start_x = 10; int start_y = 5; - int target_x = 21; + int target_x = 44; int target_y = 12; start_x = std::clamp(start_x, 0, grid_w - 1); start_y = std::clamp(start_y, 0, grid_h - 1); @@ -63,6 +64,7 @@ auto main() -> int { pathing::SplineResult result; constexpr int k_max_steps = 10000; int64_t t = 0; + int prev_closest_idx = -1; for (int step = 0; step < k_max_steps; ++step) { pathing::Point start_pt{ .x = static_cast(current_pose.X().value() / node_size_meters), @@ -76,7 +78,8 @@ auto main() -> int { target_pt.y = std::clamp(static_cast(target_pt.y), 0, grid_h - 1); frc::Translation2d t2d(target_pose.X(), target_pose.Y()); - if (current_pose.Translation().Distance(t2d).value() < node_size_meters) { + double dist_to_target = current_pose.Translation().Distance(t2d).value(); + if (dist_to_target < node_size_meters) { vx_log.Append(0.0, t); vy_log.Append(0.0, t); pose_log.Append(current_pose, t); @@ -107,13 +110,31 @@ auto main() -> int { } } + if (prev_closest_idx >= 0 && closest_idx < prev_closest_idx) { + vx_log.Append(0.0, t); + vy_log.Append(0.0, t); + pose_log.Append(current_pose, t); + break; + } + if (prev_closest_idx >= 0 && closest_idx == prev_closest_idx && + closest_idx >= static_cast(result.params.size()) - 5) { + vx_log.Append(0.0, t); + vy_log.Append(0.0, t); + pose_log.Append(current_pose, t); + break; + } + prev_closest_idx = closest_idx; + closest_idx += lookahead_; if (closest_idx >= static_cast(result.params.size())) { closest_idx = static_cast(result.params.size()) - 1; } - const auto [vx, vy] = pathing::EvaluateDerivative( + auto [vx, vy] = pathing::EvaluateDerivative( result.params[closest_idx], result.controls, result.knots, result.p, 1); + vx *= speed_; + vy *= speed_; + vx_log.Append(vx, t); vy_log.Append(vy, t); pose_log.Append(current_pose, t); From 042d6acd0c9fd674a098a06a3cef39165bcccb68 Mon Sep 17 00:00:00 2001 From: Ari Date: Tue, 5 May 2026 00:07:13 +0000 Subject: [PATCH 13/25] Add the cloest point thing to pathfinding.h Signed-off-by: Ari --- src/pathing/pathfinding.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/pathing/pathfinding.h b/src/pathing/pathfinding.h index 200d649b..e619d553 100644 --- a/src/pathing/pathfinding.h +++ b/src/pathing/pathfinding.h @@ -26,6 +26,9 @@ struct Point { uint x, y; }; +auto BFSFirstFreeCell(std::vector>& field, Point start_point) + -> Node; + auto BFS(std::vector>& field, Point start_point, Point end_point) -> std::vector; From df98587ad0ff3af89d8868e60be8d8ba3cbdb4ef Mon Sep 17 00:00:00 2001 From: Ari Date: Tue, 5 May 2026 01:15:10 +0000 Subject: [PATCH 14/25] Update nagrid.json to be the same on both sides of the field Signed-off-by: Ari --- constants/navgrid.json | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/constants/navgrid.json b/constants/navgrid.json index ffb1e806..31f0f90a 100644 --- a/constants/navgrid.json +++ b/constants/navgrid.json @@ -507,12 +507,12 @@ false, false, false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1029,12 +1029,12 @@ false, false, false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, false, false, false, From 6c0a30461044a9b1cc5c0634e92d7593bd86965b Mon Sep 17 00:00:00 2001 From: Ari Date: Tue, 5 May 2026 14:01:05 +0000 Subject: [PATCH 15/25] At the end of spline, add linear approach for more precise target position Signed-off-by: Ari --- src/pathing/simulator.cc | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 6d6a49ca..cb8a5328 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -118,11 +118,33 @@ auto main() -> int { } if (prev_closest_idx >= 0 && closest_idx == prev_closest_idx && closest_idx >= static_cast(result.params.size()) - 5) { + double dx = target_pose.X().value() - current_pose.X().value(); + double dy = target_pose.Y().value() - current_pose.Y().value(); + double dist = std::hypot(dx, dy); + + while (dist > 0.001) { + double vx = (dx / dist) * speed_; + double vy = (dy / dist) * speed_; + + vx_log.Append(vx, t); + vy_log.Append(vy, t); + pose_log.Append(current_pose, t); + + current_pose = + frc::Pose2d(units::meter_t{current_pose.X().value() + vx * dt_sec}, + units::meter_t{current_pose.Y().value() + vy * dt_sec}, + units::radian_t{0.0}); + t += dt_us; + dx = target_pose.X().value() - current_pose.X().value(); + dy = target_pose.Y().value() - current_pose.Y().value(); + dist = std::hypot(dx, dy); + } vx_log.Append(0.0, t); vy_log.Append(0.0, t); pose_log.Append(current_pose, t); break; } + prev_closest_idx = closest_idx; closest_idx += lookahead_; From 5c087dab0c9579dbfdc45c29f04b72a6aa6cc5b1 Mon Sep 17 00:00:00 2001 From: Ari Date: Tue, 5 May 2026 20:38:26 +0000 Subject: [PATCH 16/25] Even better target approaching Signed-off-by: Ari --- src/pathing/simulator.cc | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index cb8a5328..228000ac 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -14,7 +14,7 @@ auto main() -> int { const uint lookahead_ = 0; const int64_t dt_us = 20000; const double dt_sec = 0.020; - const double speed_ = 0.1; + const double speed_ = 0.2; std::ifstream file("/root/bos/constants/navgrid.json"); if (!file.is_open()) { @@ -116,15 +116,14 @@ auto main() -> int { pose_log.Append(current_pose, t); break; } - if (prev_closest_idx >= 0 && closest_idx == prev_closest_idx && - closest_idx >= static_cast(result.params.size()) - 5) { - double dx = target_pose.X().value() - current_pose.X().value(); - double dy = target_pose.Y().value() - current_pose.Y().value(); - double dist = std::hypot(dx, dy); - - while (dist > 0.001) { - double vx = (dx / dist) * speed_; - double vy = (dy / dist) * speed_; + double dx = target_pose.X().value() - current_pose.X().value(); + double dy = target_pose.Y().value() - current_pose.Y().value(); + double dist = std::hypot(dx, dy); + + if (dist <= 1) { + while (dist > 0.01) { + double vx = (dx / dist); + double vy = (dy / dist); vx_log.Append(vx, t); vy_log.Append(vy, t); From 30cdbc84f79e77a238a2e9184ddc713165911473 Mon Sep 17 00:00:00 2001 From: Ari Date: Tue, 5 May 2026 20:45:53 +0000 Subject: [PATCH 17/25] Update controller to use the same things as simulator (that is proved to work in sim) Signed-off-by: Ari --- src/pathing/controller.cc | 42 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index c05a41f1..7459ed25 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -48,6 +48,7 @@ auto RunController( auto vx_pub = inst.GetDoubleTopic("/pathing/vx").Publish(); auto vy_pub = inst.GetDoubleTopic("/pathing/vy").Publish(); SplineResult result; + int prev_closest_idx = -1; while (true) { if (!enabled_sub.Get()) { @@ -127,6 +128,46 @@ auto RunController( << " Spline size: " << result.points.size(); } + if (prev_closest_idx >= 0 && closest_idx < prev_closest_idx) { + vx_pub.Set(0.0); + vy_pub.Set(0.0); + inst.Flush(); + result.points.clear(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + if (prev_closest_idx >= 0 && closest_idx == prev_closest_idx && + closest_idx >= static_cast(result.params.size()) - 5) { + double dx = target_pose.X().value() - current_pose.X().value(); + double dy = target_pose.Y().value() - current_pose.Y().value(); + double dist = std::hypot(dx, dy); + + while (dist > 0.001) { + double vx = (dx / dist) * 1.0; + double vy = (dy / dist) * 1.0; + + vx_pub.Set(vx); + vy_pub.Set(vy); + if (verbose) { + LOG(INFO) << "linear approach vx " << vx << " vy " << vy; + } + inst.Flush(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + + current_pose = current_sub.Get(); + dx = target_pose.X().value() - current_pose.X().value(); + dy = target_pose.Y().value() - current_pose.Y().value(); + dist = std::hypot(dx, dy); + } + vx_pub.Set(0.0); + vy_pub.Set(0.0); + inst.Flush(); + result.points.clear(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + prev_closest_idx = closest_idx; + closest_idx += lookahead_; if (closest_idx >= static_cast(result.params.size())) { closest_idx = static_cast(result.params.size()) - 1; @@ -142,7 +183,6 @@ auto RunController( LOG(INFO) << "set vx " << vx << " vy " << vy; } inst.Flush(); - result = {}; } } } From 47ce39394fb0322492ed8ef5d15e94c73362b4d9 Mon Sep 17 00:00:00 2001 From: Ari Date: Wed, 6 May 2026 16:10:45 +0000 Subject: [PATCH 18/25] Better types Signed-off-by: Ari --- src/pathing/simulator.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 228000ac..85a8a85b 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -62,10 +62,10 @@ auto main() -> int { target_log.Append(target_pose, 1); pathing::SplineResult result; - constexpr int k_max_steps = 10000; + const int max_steps = 10000; int64_t t = 0; int prev_closest_idx = -1; - for (int step = 0; step < k_max_steps; ++step) { + for (int step = 0; step < max_steps; ++step) { pathing::Point start_pt{ .x = static_cast(current_pose.X().value() / node_size_meters), .y = static_cast(current_pose.Y().value() / node_size_meters)}; From 57d07a5bb6746db1ffcf6951fedb5d7da69a9b1a Mon Sep 17 00:00:00 2001 From: Ari Date: Mon, 11 May 2026 21:36:44 +0000 Subject: [PATCH 19/25] Remove redundant comment Signed-off-by: Ari --- src/pathing/controller.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 7459ed25..2b3abf5b 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -17,7 +17,6 @@ auto RunController( std::ifstream file(navgrid_path); if (!file.is_open()) { - LOG(FATAL) << "Failed to open navgrid.json" << std::endl; LOG(FATAL) << "Failed to open navgrid.json"; return; } From a4f73f2708e400f4a6266563827d40af17952b9c Mon Sep 17 00:00:00 2001 From: Ari Date: Thu, 14 May 2026 02:26:56 +0000 Subject: [PATCH 20/25] Naming conventions Signed-off-by: Ari --- src/pathing/controller.cc | 2 +- src/pathing/simulator.cc | 2 +- src/pathing/splines.cc | 10 +++++----- src/pathing/splines.h | 4 ++-- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 2b3abf5b..7d6858b1 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -87,7 +87,7 @@ auto RunController( } if (result.points.empty()) { - result = createSpline(grid, start_pt, target_pt, nodeSizeMeters); + result = CreateSpline(grid, start_pt, target_pt, nodeSizeMeters); if (!result.points.empty()) { if (verbose) { for (const auto& p : result.points) { diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 85a8a85b..d10dc16a 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -88,7 +88,7 @@ auto main() -> int { if (result.points.empty()) { result = - pathing::createSpline(grid, start_pt, target_pt, node_size_meters); + pathing::CreateSpline(grid, start_pt, target_pt, node_size_meters); } if (result.points.empty()) { diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index e612cf21..980137d8 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -56,7 +56,7 @@ auto FiniteDifferences(const std::vector>& controls, return ret; } -auto basis(int i, int p, double t, const std::vector& knots) -> double { +auto Basis(int i, int p, double t, const std::vector& knots) -> double { if (p == 0) { if (knots[i] <= t && t < knots[i + 1]) { return 1.0; @@ -68,13 +68,13 @@ auto basis(int i, int p, double t, const std::vector& knots) -> double { double left = 0.0; double denoml = knots[i + p] - knots[i]; if (denoml != 0) { - left = (t - knots[i]) / denoml * basis(i, p - 1, t, knots); + left = (t - knots[i]) / denoml * Basis(i, p - 1, t, knots); } double right = 0.0; double denomr = knots[i + p + 1] - knots[i + 1]; if (denomr != 0) { - right = (knots[i + p + 1] - t) / denomr * basis(i + 1, p - 1, t, knots); + right = (knots[i + p + 1] - t) / denomr * Basis(i + 1, p - 1, t, knots); } return left + right; @@ -91,7 +91,7 @@ auto EvaluatePosition(double t, int n = static_cast(controls.size()); double x = 0.0, y = 0.0; for (int i = 0; i < n; ++i) { - double b = basis(i, p, t, knots); + double b = Basis(i, p, t, knots); x += b * controls[i].first; y += b * controls[i].second; } @@ -117,7 +117,7 @@ auto EvaluateDerivative(double t, return {x * n, y * n}; } -auto createSpline(const std::vector>& grid, +auto CreateSpline(const std::vector>& grid, Point start_point, Point target_point, double nodeSizeMeters) -> SplineResult { diff --git a/src/pathing/splines.h b/src/pathing/splines.h index d71cb213..426b5075 100644 --- a/src/pathing/splines.h +++ b/src/pathing/splines.h @@ -23,7 +23,7 @@ auto FiniteDifferences(const std::vector>& controls, const std::vector& knots, int p, int k) -> std::vector>; -auto basis(int i, int p, double t, const std::vector& knots) -> double; +auto Basis(int i, int p, double t, const std::vector& knots) -> double; auto EvaluatePosition(double t, const std::vector>& controls, @@ -34,7 +34,7 @@ auto EvaluateDerivative(double t, const std::vector& knots, int p, int k) -> std::pair; -auto createSpline(const std::vector>& grid, Point start_point, +auto CreateSpline(const std::vector>& grid, Point start_point, Point target_point, double nodeSizeMeters) -> SplineResult; } // namespace pathing From 24e9df04d0cd4ab2bbacde70c515200dcdd64991 Mon Sep 17 00:00:00 2001 From: Ari Date: Thu, 14 May 2026 13:18:39 -0700 Subject: [PATCH 21/25] Add velocity profile Signed-off-by: Ari --- src/pathing/CMakeLists.txt | 2 +- src/pathing/controller.cc | 15 ++++--- src/pathing/simulator.cc | 13 +++--- src/pathing/velocity_profile.cc | 73 +++++++++++++++++++++++++++++++++ src/pathing/velocity_profile.h | 20 +++++++++ 5 files changed, 110 insertions(+), 13 deletions(-) create mode 100644 src/pathing/velocity_profile.cc create mode 100644 src/pathing/velocity_profile.h diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index c46eecb5..20d760d0 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -1,4 +1,4 @@ -add_library(pathing pathfinding.cc splines.cc controller.cc) +add_library(pathing pathfinding.cc splines.cc velocity_profile.cc controller.cc) target_link_libraries(pathing PUBLIC wpilibc wpiutil ntcore nlohmann_json::nlohmann_json utils localization ${OpenCV_LIBS}) add_executable(pathing_simulator simulator.cc) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 7d6858b1..8667ab1e 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -7,13 +7,14 @@ #include "src/localization/position_receiver.h" #include "src/utils/log.h" #include "src/utils/pch.h" +#include "velocity_profile.h" namespace pathing { auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { - const uint lookahead_ = 5; + const uint lookahead_ = kVelocityLookahead; std::ifstream file(navgrid_path); if (!file.is_open()) { @@ -47,6 +48,7 @@ auto RunController( auto vx_pub = inst.GetDoubleTopic("/pathing/vx").Publish(); auto vy_pub = inst.GetDoubleTopic("/pathing/vy").Publish(); SplineResult result; + std::vector> velocity_profile; int prev_closest_idx = -1; while (true) { @@ -55,6 +57,7 @@ auto RunController( vy_pub.Set(0.0); result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } @@ -82,12 +85,14 @@ auto RunController( vx_pub.Set(0.0); vy_pub.Set(0.0); result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } if (result.points.empty()) { result = CreateSpline(grid, start_pt, target_pt, nodeSizeMeters); + velocity_profile = CreateVelocityProfile(result); if (!result.points.empty()) { if (verbose) { for (const auto& p : result.points) { @@ -97,7 +102,7 @@ auto RunController( } } - if (result.points.empty()) { + if (result.points.empty() || velocity_profile.empty()) { vx_pub.Set(0.0); vy_pub.Set(0.0); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -132,6 +137,7 @@ auto RunController( vy_pub.Set(0.0); inst.Flush(); result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } @@ -162,6 +168,7 @@ auto RunController( vy_pub.Set(0.0); inst.Flush(); result.points.clear(); + velocity_profile.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } @@ -172,9 +179,7 @@ auto RunController( closest_idx = static_cast(result.params.size()) - 1; } - const auto [vx, vy] = - EvaluateDerivative(result.params[closest_idx], result.controls, - result.knots, result.p, 1); + const auto [vx, vy] = velocity_profile[closest_idx]; vx_pub.Set(vx); vy_pub.Set(vy); diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index d10dc16a..6d3d3384 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -9,12 +9,12 @@ #include #include #include "src/pathing/splines.h" +#include "src/pathing/velocity_profile.h" auto main() -> int { - const uint lookahead_ = 0; + const uint lookahead_ = pathing::kVelocityLookahead; const int64_t dt_us = 20000; const double dt_sec = 0.020; - const double speed_ = 0.2; std::ifstream file("/root/bos/constants/navgrid.json"); if (!file.is_open()) { @@ -62,6 +62,7 @@ auto main() -> int { target_log.Append(target_pose, 1); pathing::SplineResult result; + std::vector> velocity_profile; const int max_steps = 10000; int64_t t = 0; int prev_closest_idx = -1; @@ -89,9 +90,10 @@ auto main() -> int { if (result.points.empty()) { result = pathing::CreateSpline(grid, start_pt, target_pt, node_size_meters); + velocity_profile = pathing::CreateVelocityProfile(result); } - if (result.points.empty()) { + if (result.points.empty() || velocity_profile.empty()) { vx_log.Append(0.0, t); vy_log.Append(0.0, t); pose_log.Append(current_pose, t); @@ -151,10 +153,7 @@ auto main() -> int { closest_idx = static_cast(result.params.size()) - 1; } - auto [vx, vy] = pathing::EvaluateDerivative( - result.params[closest_idx], result.controls, result.knots, result.p, 1); - vx *= speed_; - vy *= speed_; + auto [vx, vy] = velocity_profile[closest_idx]; vx_log.Append(vx, t); vy_log.Append(vy, t); diff --git a/src/pathing/velocity_profile.cc b/src/pathing/velocity_profile.cc new file mode 100644 index 00000000..512c2e3a --- /dev/null +++ b/src/pathing/velocity_profile.cc @@ -0,0 +1,73 @@ +#include "velocity_profile.h" +#include +#include + +namespace pathing { + +auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, + double maxAcceleration) + -> std::vector> { + if (result.points.empty() || result.params.empty() || maxVelocity <= 0.0 || + maxAcceleration <= 0.0) { + return {}; + } + + std::vector distances; + distances.reserve(result.points.size()); + distances.emplace_back(0.0); + + for (int i = 1; i < static_cast(result.points.size()); ++i) { + frc::Translation2d prev(result.points[i - 1].X(), result.points[i - 1].Y()); + frc::Translation2d current(result.points[i].X(), result.points[i].Y()); + double distance = current.Distance(prev).value(); + distances.emplace_back(distances.back() + distance); + } + + double total_distance = distances.back(); + if (total_distance <= 0.0) { + return {}; + } + + double ramp_distance = maxVelocity * maxVelocity / (2.0 * maxAcceleration); + double profile_velocity = maxVelocity; + if (ramp_distance * 2.0 > total_distance) { + ramp_distance = total_distance / 2.0; + profile_velocity = std::sqrt(total_distance * maxAcceleration); + } + + std::vector> velocities; + velocities.reserve(result.params.size()); + + for (int i = 0; i < static_cast(result.params.size()); ++i) { + double distance = + distances[std::min(i, static_cast(distances.size()) - 1)]; + double end_distance = total_distance - distance; + double speed = profile_velocity; + + if (distance < ramp_distance) { + speed = std::sqrt(2.0 * maxAcceleration * distance); + } + if (end_distance < ramp_distance) { + speed = std::min(speed, std::sqrt(2.0 * maxAcceleration * end_distance)); + } + + auto [dx, dy] = EvaluateDerivative(result.params[i], result.controls, + result.knots, result.p, 1); + double derivative = std::hypot(dx, dy); + if (derivative <= 0.0) { + velocities.emplace_back(0.0, 0.0); + } else { + velocities.emplace_back(dx / derivative * speed, dy / derivative * speed); + } + } + + return velocities; +} + +auto CreateVelocityProfile(const SplineResult& result) + -> std::vector> { + return CreateVelocityProfile(result, kMaxProfileVelocity, + kMaxProfileAcceleration); +} + +} // namespace pathing diff --git a/src/pathing/velocity_profile.h b/src/pathing/velocity_profile.h new file mode 100644 index 00000000..4ee80631 --- /dev/null +++ b/src/pathing/velocity_profile.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#include "splines.h" + +namespace pathing { + +inline constexpr int kVelocityLookahead = 5; +inline constexpr double kMaxProfileVelocity = 1.0; +inline constexpr double kMaxProfileAcceleration = 1.0; + +auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, + double maxAcceleration) + -> std::vector>; + +auto CreateVelocityProfile(const SplineResult& result) + -> std::vector>; + +} // namespace pathing From db58165c7a75d922f7578a4baf3ce644d06fdb12 Mon Sep 17 00:00:00 2001 From: Ari Date: Thu, 14 May 2026 20:24:17 +0000 Subject: [PATCH 22/25] edits Signed-off-by: Ari --- src/pathing/controller.cc | 2 +- src/pathing/simulator.cc | 2 +- src/pathing/velocity_profile.cc | 7 +++---- src/pathing/velocity_profile.h | 6 +++--- 4 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index 8667ab1e..b1650648 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -14,7 +14,7 @@ namespace pathing { auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { - const uint lookahead_ = kVelocityLookahead; + const uint lookahead_ = velocity_lookahead; std::ifstream file(navgrid_path); if (!file.is_open()) { diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 6d3d3384..0db22497 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -12,7 +12,7 @@ #include "src/pathing/velocity_profile.h" auto main() -> int { - const uint lookahead_ = pathing::kVelocityLookahead; + const uint lookahead_ = pathing::velocity_lookahead; const int64_t dt_us = 20000; const double dt_sec = 0.020; diff --git a/src/pathing/velocity_profile.cc b/src/pathing/velocity_profile.cc index 512c2e3a..e94f87aa 100644 --- a/src/pathing/velocity_profile.cc +++ b/src/pathing/velocity_profile.cc @@ -4,8 +4,8 @@ namespace pathing { -auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, - double maxAcceleration) +auto _CreateVelocityProfile(const SplineResult& result, double maxVelocity, + double maxAcceleration) -> std::vector> { if (result.points.empty() || result.params.empty() || maxVelocity <= 0.0 || maxAcceleration <= 0.0) { @@ -66,8 +66,7 @@ auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, auto CreateVelocityProfile(const SplineResult& result) -> std::vector> { - return CreateVelocityProfile(result, kMaxProfileVelocity, - kMaxProfileAcceleration); + return _CreateVelocityProfile(result, max_velocity, max_accel); } } // namespace pathing diff --git a/src/pathing/velocity_profile.h b/src/pathing/velocity_profile.h index 4ee80631..c275bd54 100644 --- a/src/pathing/velocity_profile.h +++ b/src/pathing/velocity_profile.h @@ -6,9 +6,9 @@ namespace pathing { -inline constexpr int kVelocityLookahead = 5; -inline constexpr double kMaxProfileVelocity = 1.0; -inline constexpr double kMaxProfileAcceleration = 1.0; +const int velocity_lookahead = 5; +const double max_velocity = 1.0; +const double max_accel = 1.0; auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, double maxAcceleration) From 20d0deeab8a59a0b44e9a9ffe56f53b42a6d9ac3 Mon Sep 17 00:00:00 2001 From: Ari Date: Thu, 14 May 2026 20:27:00 +0000 Subject: [PATCH 23/25] Get constants from pathplanner and add them to velocity_profile.h Signed-off-by: Ari --- src/pathing/velocity_profile.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pathing/velocity_profile.h b/src/pathing/velocity_profile.h index c275bd54..8d5799d7 100644 --- a/src/pathing/velocity_profile.h +++ b/src/pathing/velocity_profile.h @@ -7,8 +7,9 @@ namespace pathing { const int velocity_lookahead = 5; -const double max_velocity = 1.0; -const double max_accel = 1.0; +// Gotten from current pathplanner config of team +const double max_velocity = 3.5; +const double max_accel = 3.2; auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, double maxAcceleration) From b6e5c4b9d35dfd4fde9c8366214971a65ca0f72d Mon Sep 17 00:00:00 2001 From: Ari Date: Thu, 14 May 2026 20:51:02 +0000 Subject: [PATCH 24/25] oops * renamed CreateVelocityProfile to _CreateVelocityProfile cuz i hate overloading after practicing for apcs Signed-off-by: Ari --- src/pathing/velocity_profile.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pathing/velocity_profile.h b/src/pathing/velocity_profile.h index 8d5799d7..29740fd9 100644 --- a/src/pathing/velocity_profile.h +++ b/src/pathing/velocity_profile.h @@ -11,8 +11,8 @@ const int velocity_lookahead = 5; const double max_velocity = 3.5; const double max_accel = 3.2; -auto CreateVelocityProfile(const SplineResult& result, double maxVelocity, - double maxAcceleration) +auto _CreateVelocityProfile(const SplineResult& result, double maxVelocity, + double maxAcceleration) -> std::vector>; auto CreateVelocityProfile(const SplineResult& result) From 44c699a4a2af841ca1b3019da1a7eb860128245d Mon Sep 17 00:00:00 2001 From: Ari Date: Mon, 18 May 2026 04:25:28 +0000 Subject: [PATCH 25/25] Fix velocity profile * start from index 1 in velocity profile as index 0 has velocities 0, we are stuck * remove lookahead fully, sa it was only needed before the derivatives when we calculated direction from a difference * this caused us to jump to a high speed (1.5 m/s) immediately after starting * reduced spline degree to 3 for less cuts * need to clean up code!!! Signed-off-by: Ari --- src/pathing/CMakeLists.txt | 2 +- src/pathing/controller.cc | 2 -- src/pathing/simulator.cc | 63 +++++++++++++-------------------- src/pathing/splines.cc | 2 +- src/pathing/velocity_profile.cc | 5 +-- src/pathing/velocity_profile.h | 1 - 6 files changed, 30 insertions(+), 45 deletions(-) diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt index 20d760d0..605cee8c 100644 --- a/src/pathing/CMakeLists.txt +++ b/src/pathing/CMakeLists.txt @@ -2,4 +2,4 @@ add_library(pathing pathfinding.cc splines.cc velocity_profile.cc controller.cc) target_link_libraries(pathing PUBLIC wpilibc wpiutil ntcore nlohmann_json::nlohmann_json utils localization ${OpenCV_LIBS}) add_executable(pathing_simulator simulator.cc) -target_link_libraries(pathing_simulator PRIVATE pathing) +target_link_libraries(pathing_simulator PRIVATE pathing utils) diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc index b1650648..899a4106 100644 --- a/src/pathing/controller.cc +++ b/src/pathing/controller.cc @@ -14,7 +14,6 @@ namespace pathing { auto RunController( const std::string& navgrid_path = "/root/bos/constants/navgrid.json", bool verbose = false) -> void { - const uint lookahead_ = velocity_lookahead; std::ifstream file(navgrid_path); if (!file.is_open()) { @@ -174,7 +173,6 @@ auto RunController( } prev_closest_idx = closest_idx; - closest_idx += lookahead_; if (closest_idx >= static_cast(result.params.size())) { closest_idx = static_cast(result.params.size()) - 1; } diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc index 0db22497..dcd083bc 100644 --- a/src/pathing/simulator.cc +++ b/src/pathing/simulator.cc @@ -10,9 +10,9 @@ #include #include "src/pathing/splines.h" #include "src/pathing/velocity_profile.h" +#include "src/utils/log.h" auto main() -> int { - const uint lookahead_ = pathing::velocity_lookahead; const int64_t dt_us = 20000; const double dt_sec = 0.020; @@ -38,7 +38,7 @@ auto main() -> int { } int start_x = 10; - int start_y = 5; + int start_y = 6; int target_x = 44; int target_y = 12; start_x = std::clamp(start_x, 0, grid_w - 1); @@ -59,13 +59,17 @@ auto main() -> int { wpi::log::StructLogEntry target_log(log, "/sim/TargetPose2d"); wpi::log::DoubleLogEntry vx_log(log, "/sim/pathing/vx"); wpi::log::DoubleLogEntry vy_log(log, "/sim/pathing/vy"); + wpi::log::DoubleLogEntry speed_log(log, "/sim/speed"); + wpi::log::IntegerLogEntry closest_idx_log(log, "/sim/closest_idx"); + wpi::log::DoubleLogEntry v_profile_zero_x(log, "/sim/v_profile_zero_x"); + wpi::log::DoubleLogEntry v_profile_zero_y(log, "/sim/v_profile_zero_y"); target_log.Append(target_pose, 1); pathing::SplineResult result; std::vector> velocity_profile; const int max_steps = 10000; int64_t t = 0; - int prev_closest_idx = -1; + int prev_closest_idx = 0; for (int step = 0; step < max_steps; ++step) { pathing::Point start_pt{ .x = static_cast(current_pose.X().value() / node_size_meters), @@ -91,6 +95,7 @@ auto main() -> int { result = pathing::CreateSpline(grid, start_pt, target_pt, node_size_meters); velocity_profile = pathing::CreateVelocityProfile(result); + prev_closest_idx = -1; } if (result.points.empty() || velocity_profile.empty()) { @@ -100,10 +105,19 @@ auto main() -> int { break; } - int closest_idx = 0; - frc::Translation2d first2d(result.points[0].X(), result.points[0].Y()); + const int search_window = 1000; + + // Start searching from 1 because index 0 of velocity profile is always 0, + // it needs an initial speed. + int start_search = std::max(1, prev_closest_idx); + int end_search = std::min(static_cast(result.points.size()) - 1, + start_search + search_window); + + int closest_idx = start_search; + frc::Translation2d first2d(result.points[start_search].X(), + result.points[start_search].Y()); double best_dist = current_pose.Translation().Distance(first2d).value(); - for (int i = 1; i < static_cast(result.points.size()); ++i) { + for (int i = start_search + 1; i <= end_search; ++i) { frc::Translation2d p(result.points[i].X(), result.points[i].Y()); double d = current_pose.Translation().Distance(p).value(); if (d < best_dist) { @@ -113,42 +127,11 @@ auto main() -> int { } if (prev_closest_idx >= 0 && closest_idx < prev_closest_idx) { - vx_log.Append(0.0, t); - vy_log.Append(0.0, t); - pose_log.Append(current_pose, t); - break; - } - double dx = target_pose.X().value() - current_pose.X().value(); - double dy = target_pose.Y().value() - current_pose.Y().value(); - double dist = std::hypot(dx, dy); - - if (dist <= 1) { - while (dist > 0.01) { - double vx = (dx / dist); - double vy = (dy / dist); - - vx_log.Append(vx, t); - vy_log.Append(vy, t); - pose_log.Append(current_pose, t); - - current_pose = - frc::Pose2d(units::meter_t{current_pose.X().value() + vx * dt_sec}, - units::meter_t{current_pose.Y().value() + vy * dt_sec}, - units::radian_t{0.0}); - t += dt_us; - dx = target_pose.X().value() - current_pose.X().value(); - dy = target_pose.Y().value() - current_pose.Y().value(); - dist = std::hypot(dx, dy); - } - vx_log.Append(0.0, t); - vy_log.Append(0.0, t); - pose_log.Append(current_pose, t); - break; + closest_idx = prev_closest_idx; } prev_closest_idx = closest_idx; - closest_idx += lookahead_; if (closest_idx >= static_cast(result.params.size())) { closest_idx = static_cast(result.params.size()) - 1; } @@ -157,7 +140,11 @@ auto main() -> int { vx_log.Append(vx, t); vy_log.Append(vy, t); + speed_log.Append(std::hypot(vx, vy), t); + closest_idx_log.Append(closest_idx, t); pose_log.Append(current_pose, t); + v_profile_zero_x.Append(velocity_profile[0].first, t); + v_profile_zero_y.Append(velocity_profile[0].second, t); current_pose = frc::Pose2d(units::meter_t{current_pose.X().value() + vx * dt_sec}, diff --git a/src/pathing/splines.cc b/src/pathing/splines.cc index 980137d8..39df2cf6 100644 --- a/src/pathing/splines.cc +++ b/src/pathing/splines.cc @@ -146,7 +146,7 @@ auto CreateSpline(const std::vector>& grid, return {}; } - p = 4; + p = 3; if (numControls <= p) { p = numControls - 1; } diff --git a/src/pathing/velocity_profile.cc b/src/pathing/velocity_profile.cc index e94f87aa..2f39515e 100644 --- a/src/pathing/velocity_profile.cc +++ b/src/pathing/velocity_profile.cc @@ -54,10 +54,11 @@ auto _CreateVelocityProfile(const SplineResult& result, double maxVelocity, auto [dx, dy] = EvaluateDerivative(result.params[i], result.controls, result.knots, result.p, 1); double derivative = std::hypot(dx, dy); - if (derivative <= 0.0) { + if (derivative <= 1e-6) { velocities.emplace_back(0.0, 0.0); } else { - velocities.emplace_back(dx / derivative * speed, dy / derivative * speed); + velocities.emplace_back((dx / derivative) * speed, + (dy / derivative) * speed); } } diff --git a/src/pathing/velocity_profile.h b/src/pathing/velocity_profile.h index 29740fd9..6206f247 100644 --- a/src/pathing/velocity_profile.h +++ b/src/pathing/velocity_profile.h @@ -6,7 +6,6 @@ namespace pathing { -const int velocity_lookahead = 5; // Gotten from current pathplanner config of team const double max_velocity = 3.5; const double max_accel = 3.2;