From 6b7273f45b0ee805b71a7665144e487c06d7a5b3 Mon Sep 17 00:00:00 2001 From: Johannes Reinhardt Date: Sun, 13 Jul 2008 19:42:36 +0200 Subject: [PATCH] fixed some things on new simulation --- src/lookahead.c | 82 ++++++++++++++++++++++++++++++------------------ src/radar_main.c | 12 ++++--- 2 files changed, 60 insertions(+), 34 deletions(-) diff --git a/src/lookahead.c b/src/lookahead.c index ea74a6d..03f58b2 100644 --- a/src/lookahead.c +++ b/src/lookahead.c @@ -2,50 +2,78 @@ #include #include + +double turn2w(trial* t, turn_t turn){ + switch(turn){ + case(TURN_HARD_LEFT): return t->map.max_hard_turn/1000.0; + case(TURN_LEFT): return t->map.max_turn/1000.0; + case(TURN_STRAIGHT): return 0; + case(TURN_RIGHT): return -t->map.max_turn/1000.0; + case(TURN_HARD_RIGHT): return -t->map.max_hard_turn/1000.0; + fprintf(stderr,"Unknown turn state in turn2w\n"); + return 0; + } +} + struct collcheck { int collision; double x,y; }; void check_coll(gpointer key, gpointer value, gpointer ud){ + struct collcheck* coll; + object* ob; + + coll = (struct collcheck*) ud; + ob = (object*) value; + double dx,dy; - dx = value.x - ud.x; - dy = value.y - ud.y; - *ud.collision |= (sqrt(dx*dx + dy*dy) - value.rad) < 1; + dx = ob->x - coll->x; + dy = ob->y - coll->y; + coll->collision |= (sqrt(dx*dx + dy*dy) - ob->rad) < 1; } void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){ double a1,a2; double w1,w2; double dt1,dt2; + vehicle *v1, *v2, *v3; + + v1 = &t1->vehicle; + v2 = &t2->vehicle; + v3 = &t3->vehicle; dt1 = (t2->ts - t1->ts); dt2 = (t3->ts - t2->ts); - a1 = (t2->speed - t1->speed)/dt1; - a2 = (t3->speed - t2->speed)/dt2; + a1 = (v2->speed - v1->speed)/dt1; + a2 = (v3->speed - v2->speed)/dt2; if(a1/a2 > 1.1 || a1/a2 < 0.9){ - fprintf(stderr,"Inkonsistent accelerations, inaccurate results possible\n"); + fprintf(stderr,"Inconsistent accelerations: a1: %f a2: %f. inaccurate results possible\n",a1,a2); } m->a = (a1 + a2)/2; m->k = m->a/m->max_speed/m->max_speed; - w1 = (t2->dir - t1->dir)/dt1; - w2 = (t3->dir - t2->dir)/dt2; + w1 = (v2->dir - v1->dir)/dt1; + w2 = (v3->dir - v2->dir)/dt2; m->aw = 2*(w2 - w1)/(dt1 + dt2); } -void dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){ - double a,k; +int dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){ + double a,k,aw; + double wsoll; vehicle tmp = *before; vehicle diff; timestamp dt = 10; int i,end=deltat/dt; struct collcheck collision; + if(deltat < dt) + return 0; + if((deltat/dt)*dt != deltat) fprintf(stderr,"deltat not a multiple of %d, possibly inaccurate results\n",dt); @@ -53,22 +81,16 @@ void dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){ 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; + + wsoll = turn2w(t,before->turn); + aw *= (before->w > wsoll) ? -1 : 1; for(i = 0;imap.solid->objects,check_coll,&collision); - if(collision){ - return -1; + g_hash_table_foreach(t->map.solid_objects,check_coll,&collision); + if(collision.collision){ + return -1; } } - after.x = tmp.x; - after.y = tmp.y; - after.speed = tmp.speed; - after.w = tmp.w; - after.dir = tmp.dir; - + after->x = tmp.x; + after->y = tmp.y; + after->speed = tmp.speed; + after->w = tmp.w; + after->dir = tmp.dir; + return 0; } diff --git a/src/radar_main.c b/src/radar_main.c index 5a39da5..8c28205 100644 --- a/src/radar_main.c +++ b/src/radar_main.c @@ -20,17 +20,21 @@ void trial_loop(trial *t) { vehicle_hard_right(t); break; default: break; } - while (t->telemetry.length < 4) { + while (t->telemetry.length < 5) { if (-1 == trial_wait_for_input(t)) return; } - fit_parameter( (telemetry*) g_queue_peek_nth(&t->telemetry,1), - (telemetry*) g_queue_peek_nth(&t->telemetry,2), + fit_parameter( (telemetry*) g_queue_peek_nth(&t->telemetry,2), (telemetry*) g_queue_peek_nth(&t->telemetry,3), + (telemetry*) g_queue_peek_nth(&t->telemetry,4), &t->map); + fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k); } while (t->alive) { - simulate(t, getcurts(t) + LATENCY); + t->sim.tm.vehicle.w = turn2w(t->sim.tm.vehicle.turn); + dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,getcurts(t) + LATENCY - t->sim.tm.ts); + t->sim.steps++; +// simulate(t, getcurts(t) + LATENCY); goradar(t); if (-1 == trial_check_input(t)) return; // if (-1 == trial_wait_for_input(t)) return;