fixed some things on new simulation

This commit is contained in:
Johannes Reinhardt 2008-07-13 19:42:36 +02:00
parent a997772874
commit 6b7273f45b
2 changed files with 60 additions and 34 deletions

View File

@ -2,50 +2,78 @@
#include <stdio.h>
#include <math.h>
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;i<end;i++){
/*calculate derivative*/
diff.x = cos(tmp.dir)*tmp.speed;
diff.y = sin(tmp.dir)*tmp.speed;
diff.x = cos(tmp.dir)*tmp.speed/1000;
diff.y = sin(tmp.dir)*tmp.speed/1000;
diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed);
/*maybe its logistic: k*w*(wsoll - w)*/
diff.w = (tmp.w == wsoll) ? 0 : aw;
diff.dir = tmp.w;
/*Euler Step*/
@ -82,19 +104,19 @@ void dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
collision.x = tmp.x;
collision.y = tmp.y;
collision.collision = 0;
g_hash_foreach(t->map.solid->objects,check_coll,&collision);
if(collision){
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;
}

View File

@ -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;