| |
| #include <cmath> |
| #include <cstdio> |
| #include <vector> |
|
|
| #include "drive.hpp" |
|
|
| int main() { |
| std::vector<std::array<float, 2>> path; |
| for (float x = 0; x < 40; x += 0.5f) path.push_back({x, 0}); |
| for (float a = 0; a <= 1.5708f; a += 0.02f) path.push_back({40 + 25 * std::sin(a), 25 - 25 * std::cos(a)}); |
| for (float y = 25; y < 60; y += 0.5f) path.push_back({65, y}); |
| vkgs::Vehicle car; |
| size_t idx = 0; |
| float worst = 0, vmax = 0; |
| for (int k = 0; k < 4000; ++k) { |
| vkgs::VehicleControl c = vkgs::follow_path(car, path, idx); |
| car.step(c, 0.01f); |
| if (k > 300) worst = std::max(worst, vkgs::distance_to_polyline(path, car.x, car.y)); |
| vmax = std::max(vmax, car.v); |
| } |
| float end = std::hypot(car.x - 65, car.y - 60); |
| std::printf("worst %.2f m, vmax %.1f km/h, end %.1f m, v %.2f\n", worst, vmax * 3.6f, end, car.v); |
| bool ok = worst < 0.6f && vmax > 15 / 3.6f && vmax < 21 / 3.6f && end < 9 && std::abs(car.v) < 0.1f; |
| return ok ? 0 : 1; |
| } |
|
|
