started a new algorithm

extended map with fit parameters
master
Johannes Reinhardt 14 years ago
parent 07830c6bbd
commit 65c06c48e7
  1. 10
      src/control.h
  2. 36
      src/lookahead-main.c
  3. 114
      src/lookahead.c

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

@ -0,0 +1,36 @@
#include "control.h"
#include <stdio.h>
#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;
}

@ -0,0 +1,114 @@
#include "control.h"
#include <stdio.h>
#include <math.h>
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<end;i++){
/*calculate derivative*/
diff.x = cos(tmp.dir)*tmp.speed;
diff.y = sin(tmp.dir)*tmp.speed;
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*/
tmp.x += dt*diff.x;
tmp.y += dt*diff.y;
tmp.speed += (tmp.speed > 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,&current,20) == -1) continue;
/*TODO: Bewertung*/
}
}
}
Loading…
Cancel
Save