File size: 1,151 Bytes
4c75661 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 | // drive.hpp follow_path (자동 주행) — 직선 뒤 반지름 25 m 왼쪽 90° 커브를 따라가 끝에서 멈추는지
#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) { // 10 ms × 40 s
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;
}
|