diff --git a/src/control.h b/src/control.h index a2c257f..b246ac7 100644 --- a/src/control.h +++ b/src/control.h @@ -55,6 +55,8 @@ struct vehicle { turn_t turn; double x, y; double dir, speed; + /*Winkelgeschwindigkeit*/ + double w; }; struct telemetry { @@ -64,10 +66,12 @@ struct telemetry { }; struct map { - float dx, dy; + double dx, dy; timestamp limit; - float min_sensor, max_sensor; - float max_speed, max_turn, max_hard_turn; + double min_sensor, max_sensor; + double max_speed, max_turn, max_hard_turn; + /*unknown quantities: acceleration, angle acceleration, drag coefficient*/ + double a, aw, k; GArray *solid_objects; }; diff --git a/src/lookahead-main.c b/src/lookahead-main.c new file mode 100644 index 0000000..3f4beef --- /dev/null +++ b/src/lookahead-main.c @@ -0,0 +1,36 @@ +#include "control.h" + +#include + +#define LATENCY 20 + +void trial_loop(trial *t) { + do { + if (-1 == trial_wait_for_start(t)) return; + if (t->finished) break; + while (t->alive) { + lookahead(t); + if (-1 == trial_check_input(t)) return; + } + trial_reset_run(t); + } while (!t->finished); +} + +int main(int argc, char **argv) { + trial *t; + + if (argc <= 2) { + fprintf(stderr, "Syntax: %s hostname port\n", argv[0]); + return 1; + } + + if (NULL == (t = trial_new(argv[1], argv[2]))) { + return 2; + } + + trial_loop(t); + + trial_free(t); + + return 0; +} diff --git a/src/lookahead.c b/src/lookahead.c new file mode 100644 index 0000000..86fd7ee --- /dev/null +++ b/src/lookahead.c @@ -0,0 +1,114 @@ +#include "control.h" +#include +#include + +struct collcheck { + int collision; + double x,y; +}; + +void check_coll(gpointer key, gpointer value, gpointer ud){ + double dx,dy; + dx = value.x - ud.x; + dy = value.y - ud.y; + *ud.collision |= (sqrt(dx*dx + dy*dy) - value.rad) < 1; +} + +void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, double* a, double* aw){ + double a1,a2; + double w1,w2; + double dt1,dt2; + + dt1 = (t2->ts - t1->ts); + dt2 = (t3->ts - t2->ts); + + a1 = (t2->speed - t1->speed)/dt1; + a2 = (t3->speed - t2->speed)/dt2; + + if(a1/a2 > 1.1 || a1/a2 < 0.9){ + fprintf(stderr,"Inkonsistent accelerations, inaccurate results possible\n"); + } + + *a = (a1 + a2)/2; + + w1 = (t2->dir - t1->dir)/dt1; + w2 = (t3->dir - t2->dir)/dt2; + + *aw = 2*(w2 - w1)/(dt1 + dt2); +} + +void dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){ + double a,k; + vehicle tmp = *before; + vehicle diff; + timestamp dt = 10; + int i,end=deltat/dt; + struct collcheck collision; + + if((deltat/dt)*dt != deltat) + fprintf(stderr,"deltat not a multiple of %d, possibly inaccurate results\n",dt); + + /*TODO: find out a,k,aw*/ + a = t->map.a; + k = t->map.k; + aw = t->map.aw; + case(before.turn){ + case(TURN_HARD_LEFT): wsoll = t->map.max_hard_turn; break; + case(TURN_LEFT): wsoll = t->map.max_turn; break; + case(TURN_STRAIGHT): wsoll = 0; break; + case(TURN_RIGHT): wsoll = -t->map.max_turn; break; + case(TURN_HARD_RIGHT): wsoll = -t->map.max_hard_turn; break; + } + aw *= (w > wsoll) ? -1 : 1; + + for(i = 0;i 0) ? dt*diff.speed : 0; + tmp.w += dt*diff.w; + tmp.dir = dt*diff.dir; + + /*check for collisions TODO: optimize*/ + collision.x = tmp.x; + collision.y = tmp.y; + collision.collision = 0; + g_hash_foreach(t->map.solid->objects,check_coll,&collision); + if(collision){ + return -1; + } + + } + + after.x = tmp.x; + after.y = tmp.y; + after.speed = tmp.speed; + after.w = tmp.w; + after.dir = tmp.dir; + +} + + +void lookahead(trial* t){ + int accel, turn; + vehicle current, future; + + current = t->vehicle; + + for(accel=0; accel < 3;accel++){ + current.accel = accel; + for(turn=0; turn < 5; turn++){ + current.turn = turn; + if(dgl(t,&future,¤t,20) == -1) continue; + /*TODO: Bewertung*/ + } + } +}