fixed some things on new simulation
This commit is contained in:
Johannes Reinhardt
parent
a997772874
commit
6b7273f45b
|
|
@ -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){
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue