mirror of
https://github.com/Team4388/auto_planner.git
synced 2026-06-09 00:38:06 -06:00
Add files via upload
This commit is contained in:
66945
+316
@@ -0,0 +1,316 @@
|
||||
#include "planner.h"
|
||||
#include "../gfx/gfx.h"
|
||||
#include "../include/SDL.h"
|
||||
#include "math/bezier.h"
|
||||
#include "math/mathutils.h"
|
||||
#include "motion.h"
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#define SELECTED state.paths.arr[state.paths.selected]
|
||||
|
||||
#define STATE_FIELD_RESCALED state.field.w_meters, state.field.h_meters, 0, 0
|
||||
|
||||
static PlannerState state;
|
||||
static BezierPath bezier = {
|
||||
.arr = NULL,
|
||||
.cap = 51,
|
||||
.len = 0,
|
||||
.selected = -1,
|
||||
};
|
||||
|
||||
const point START_POSITIONS[6] = {
|
||||
[RED_ONE] = {14.5, 3.6},
|
||||
[RED_TWO] = {14.5, 5.5},
|
||||
[RED_THREE] = {14.5, 7.0},
|
||||
|
||||
[BLUE_ONE] = {2.0, 3.6},
|
||||
[BLUE_TWO] = {2.0, 5.5},
|
||||
[BLUE_THREE] = {2.0, 7.0},
|
||||
};
|
||||
|
||||
static void build_bounding_box(MultiPath *path) {
|
||||
rect box = {
|
||||
state.field.startx,
|
||||
state.field.starty,
|
||||
state.field.startx,
|
||||
state.field.starty
|
||||
};
|
||||
|
||||
if (path->type == PATH_MOTION) {
|
||||
printf("motion.len: %d\n", path->motion.len);
|
||||
for (int i = 0; i < path->motion.len; i++) {
|
||||
double x = path->motion.odo_path[i].x;
|
||||
double y = path->motion.odo_path[i].y;
|
||||
|
||||
if (x < box.x) box.x = x;
|
||||
if (y < box.y) box.y = y;
|
||||
if (x > box.w) box.w = x;
|
||||
if (y > box.h) box.h = y;
|
||||
}
|
||||
} else {
|
||||
for (int i = 0; i < path->bezier.len; i += 2) {
|
||||
point prev = path->bezier.arr[i];
|
||||
|
||||
for (float j = 0; j < 1.f; j += RES) {
|
||||
point percent_a = lerp_point(path->bezier.arr[i],
|
||||
path->bezier.arr[i + 1], j);
|
||||
|
||||
point percent_b = lerp_point(path->bezier.arr[i + 1],
|
||||
path->bezier.arr[i + 2], j);
|
||||
|
||||
point curve = lerp_point(percent_a, percent_b, j);
|
||||
if (curve.x < box.x) box.x = curve.x;
|
||||
if (curve.y < box.y) box.y = curve.y;
|
||||
if (curve.x > box.w) box.w = curve.x;
|
||||
if (curve.y > box.h) box.h = curve.y;
|
||||
|
||||
prev = curve;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
point ll = scale_to_screen(box.x, box.y, STATE_FIELD_RESCALED);
|
||||
point rr = scale_to_screen(box.w - box.x, box.h - box.y, STATE_FIELD_RESCALED);
|
||||
path->bounding_box = (rect) {ll.x, ll.y, rr.x, rr.y};
|
||||
}
|
||||
|
||||
void init_planner(StartPos start) {
|
||||
const size_t path_arr_size = sizeof(MotionPath) * 10;
|
||||
|
||||
state = (PlannerState) {
|
||||
.field = {
|
||||
.startx = START_POSITIONS[start].x, // 8.25,
|
||||
.starty = START_POSITIONS[start].y, // 4.05,
|
||||
.w_meters = 16.5,
|
||||
.h_meters = 8.1,
|
||||
.xy_speed = 0.8, //4.8,
|
||||
.rot_speed = 0,
|
||||
},
|
||||
.zoom_level = 1,
|
||||
.x_offset = 0,
|
||||
.y_offset = 0,
|
||||
|
||||
.paths = {
|
||||
.cap = 4,
|
||||
.len = 0,
|
||||
.arr = malloc(path_arr_size),
|
||||
.selected = 0,
|
||||
},
|
||||
|
||||
.mode = MODE_EDIT,
|
||||
};
|
||||
|
||||
memset(state.paths.arr, 0, path_arr_size);
|
||||
|
||||
if (state.paths.arr == NULL) {
|
||||
printf("could not allocate MotionPaths");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
const size_t bezier_size = sizeof(point) * bezier.cap;
|
||||
bezier.arr = malloc(bezier_size);
|
||||
memset(bezier.arr, 0, bezier_size);
|
||||
|
||||
bezier.arr[bezier.len++] = (point) {
|
||||
state.field.startx,
|
||||
state.field.starty
|
||||
};
|
||||
|
||||
bezier.arr[bezier.len++] = (point) {
|
||||
state.field.startx + 1,
|
||||
state.field.starty
|
||||
};
|
||||
|
||||
bezier.arr[bezier.len++] = (point) {
|
||||
state.field.startx + 2,
|
||||
state.field.starty
|
||||
};
|
||||
|
||||
state.paths.arr[state.paths.len++] = WRAP_BEZIER(bezier);
|
||||
state.paths.arr[state.paths.len++] =
|
||||
WRAP_MOTION(load_path("bezier.txt", &state.field));
|
||||
|
||||
for (int i = 0; i < state.paths.len; i++)
|
||||
build_bounding_box(&state.paths.arr[i]);
|
||||
}
|
||||
|
||||
static void path_playback(bool start, MotionPath *path) {
|
||||
static clock_t start_time = 0;
|
||||
static int index = 1;
|
||||
|
||||
if (start) {
|
||||
start_time = clock();
|
||||
index = 1;
|
||||
}
|
||||
|
||||
clock_t playback_time = clock() - start_time;
|
||||
|
||||
while (index < path->len && path->points[index].time < playback_time) {
|
||||
index++;
|
||||
}
|
||||
|
||||
if (index == path->len) {
|
||||
state.mode = MODE_EDIT;
|
||||
return;
|
||||
}
|
||||
|
||||
double x_min = path->odo_path[index - 1].x;
|
||||
double x_max = path->odo_path[index ].x;
|
||||
|
||||
double y_min = path->odo_path[index - 1].y;
|
||||
double y_max = path->odo_path[index ].y;
|
||||
|
||||
double lerp_time =
|
||||
((double) (path->points[index].time - path->points[index-1].time)) /
|
||||
((double) playback_time);
|
||||
|
||||
point robot_pos = {
|
||||
x_min + lerp_time * (x_max - x_min),
|
||||
y_min + lerp_time * (y_max - y_min),
|
||||
};
|
||||
|
||||
draw_robot(&state.field, robot_pos);
|
||||
}
|
||||
|
||||
static void bezier_playback(bool start, BezierPath *path) {
|
||||
|
||||
}
|
||||
|
||||
// TODO: use BezierPath & port to bezier module
|
||||
static void deselect_bez_point(MultiPath *path) {
|
||||
if (path->type == PATH_MOTION) return;
|
||||
path->bezier.selected = -1;
|
||||
build_bounding_box(path);
|
||||
}
|
||||
|
||||
static void select_path(int x, int y) {
|
||||
point mouse = { x, y };
|
||||
|
||||
for (int i = 0; i < state.paths.len; i++) {
|
||||
if (within_box(mouse, state.paths.arr[i].bounding_box)) {
|
||||
state.paths.selected = i;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void draw_correct_path(MultiPath *path, bool bold, bool select) {
|
||||
if (path->type == PATH_MOTION)
|
||||
draw_path(&path->motion, &state.field, bold, select);
|
||||
else
|
||||
draw_bezier(&path->bezier, &state.field, bold, select);
|
||||
}
|
||||
|
||||
static void draw_paths(int x, int y) {
|
||||
point mouse = { x, y };
|
||||
|
||||
for (int i = 0; i < state.paths.len && i < 2; i++) {
|
||||
if (state.paths.selected == i) {
|
||||
draw_correct_path(&state.paths.arr[i], true, true);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (within_box(mouse, state.paths.arr[i].bounding_box))
|
||||
draw_correct_path(&state.paths.arr[i], true, false);
|
||||
else
|
||||
draw_correct_path(&state.paths.arr[i], false, false);
|
||||
}
|
||||
}
|
||||
|
||||
void save_correct_format(MultiPath *path) {
|
||||
if (path->type == PATH_MOTION)
|
||||
save_path(&path->motion);
|
||||
else
|
||||
save_bezier(&path->bezier, "bezier.txt", state.field.xy_speed);
|
||||
}
|
||||
|
||||
void planner_loop(SDL_Event *e) {
|
||||
int x, y;
|
||||
SDL_GetMouseState(&x, &y);
|
||||
|
||||
while (SDL_PollEvent(e)) {
|
||||
switch (e->type) {
|
||||
case SDL_QUIT:
|
||||
// FIXME: crashing the program to exit is rather ungraceful.
|
||||
// I should make a cleanup procedure at some point.
|
||||
exit(0);
|
||||
break;
|
||||
|
||||
case SDL_MOUSEBUTTONDOWN:
|
||||
if (SELECTED.type == PATH_BEZIER &&
|
||||
select_bez_point(x, y, &SELECTED.bezier, &state.field)) break;
|
||||
|
||||
select_path(x, y);
|
||||
break;
|
||||
|
||||
case SDL_MOUSEBUTTONUP:
|
||||
deselect_bez_point(&SELECTED);
|
||||
break;
|
||||
|
||||
case SDL_KEYDOWN:
|
||||
switch(e->key.keysym.scancode) {
|
||||
case SDL_SCANCODE_P:
|
||||
if (state.paths.selected >= 0 && SELECTED.type == PATH_MOTION) {
|
||||
path_playback(true, &SELECTED.motion);
|
||||
state.mode = MODE_PLAYBACK;
|
||||
}
|
||||
break;
|
||||
|
||||
case SDL_SCANCODE_A:
|
||||
if (state.paths.selected >= 0 && SELECTED.type == PATH_BEZIER) {
|
||||
if (bezier.len >= bezier.cap) break;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
SELECTED.bezier.arr[SELECTED.bezier.len++] = (point) {
|
||||
.x = state.field.startx,
|
||||
.y = state.field.starty,
|
||||
};
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case SDL_SCANCODE_S:
|
||||
save_correct_format(&SELECTED);
|
||||
break;
|
||||
|
||||
case SDL_SCANCODE_D:
|
||||
printf("(%f, %f)\n", bezier.arr[0].x, bezier.arr[0].y);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
draw_field(&state.field);
|
||||
|
||||
switch (state.mode) {
|
||||
case MODE_EDIT:
|
||||
if (SELECTED.type == PATH_BEZIER)
|
||||
move_point(x, y, &SELECTED.bezier, &state.field);
|
||||
|
||||
draw_paths(x, y);
|
||||
draw_robot(&state.field, (point) { state.field.startx, state.field.starty });
|
||||
break;
|
||||
case MODE_PLAYBACK:
|
||||
DRAW_PATH_SELECT(&SELECTED.motion, &state.field);
|
||||
path_playback(false, &SELECTED.motion);
|
||||
break;
|
||||
case MODE_RECORD:
|
||||
DRAW_PATH_SELECT(&SELECTED.motion, &state.field);
|
||||
draw_robot(&state.field, (point) {
|
||||
SELECTED.motion.odo_path[SELECTED.motion.len - 1].x,
|
||||
SELECTED.motion.odo_path[SELECTED.motion.len - 1].y,
|
||||
});
|
||||
break;
|
||||
}
|
||||
|
||||
blit_screen();
|
||||
}
|
||||
Reference in New Issue
Block a user