This commit is contained in:
Stefan Bühler 2008-07-12 17:50:24 +02:00
parent 778d9ca994
commit d37d09ee66
7 changed files with 110 additions and 48 deletions

View File

@ -31,6 +31,9 @@ typedef struct vehicle vehicle;
struct telemetry; struct telemetry;
typedef struct telemetry telemetry; typedef struct telemetry telemetry;
struct simulated;
typedef struct simulated simulated;
struct map; struct map;
typedef struct map map; typedef struct map map;
@ -69,19 +72,30 @@ struct map {
GArray *solid_objects; GArray *solid_objects;
}; };
struct simulated {
timestamp ts;
vehicle vehicle;
};
struct trial { struct trial {
map map; map map;
timestamp last_ts; timestamp last_ts;
GQueue telemetry; GQueue telemetry;
vehicle vehicle; /* our view */ vehicle vehicle;
int alive, finished; int alive, finished;
struct timeval started;
simulated sim;
/* internal */ /* internal */
struct timeval started;
int socket; int socket;
control_parser_ctx *parse_ctx; control_parser_ctx *parse_ctx;
}; };
/* extern */
void simulate(trial *t);
void godown(trial *t);
/* trial */ /* trial */
trial *trial_new(const char *hostname, const char *port); trial *trial_new(const char *hostname, const char *port);
void trial_reset_run(trial *t); void trial_reset_run(trial *t);
@ -116,7 +130,10 @@ telemetry* telemetry_new();
/* Inline functions */ /* Inline functions */
#define SENDCMD(t, cmd) write(t->socket, cmd, sizeof(cmd)-1) #define SENDCMD(t, cmd) do { \
write(t->socket, cmd, sizeof(cmd)-1); \
/* write(1, cmd, sizeof(cmd)-1); */ \
} while(0)
INLINE void vehicle_accel(trial *t) { INLINE void vehicle_accel(trial *t) {
switch (t->vehicle.accel) { switch (t->vehicle.accel) {

View File

@ -27,7 +27,7 @@ static timestamp extract_ts(context *ctx, char *fpc) {
return atoi(ctx->tmp->str); return atoi(ctx->tmp->str);
} }
#line 130 "control_parser.rl" #line 134 "control_parser.rl"
@ -243,7 +243,7 @@ static const int control_parser_error = 0;
static const int control_parser_en_main = 1; static const int control_parser_en_main = 1;
#line 133 "control_parser.rl" #line 137 "control_parser.rl"
static int control_parser_has_error(context *ctx) { static int control_parser_has_error(context *ctx) {
return ctx->cs == control_parser_error; return ctx->cs == control_parser_error;
@ -260,7 +260,7 @@ void control_parser_new(trial *t) {
{ {
( ctx->cs) = control_parser_start; ( ctx->cs) = control_parser_start;
} }
#line 145 "control_parser.rl" #line 149 "control_parser.rl"
ctx->buffer = g_string_sized_new(0); ctx->buffer = g_string_sized_new(0);
ctx->tmp = g_string_sized_new(0); ctx->tmp = g_string_sized_new(0);
ctx->mark = -1; ctx->mark = -1;
@ -274,7 +274,7 @@ void control_parser_reset(trial *t) {
{ {
( ctx->cs) = control_parser_start; ( ctx->cs) = control_parser_start;
} }
#line 154 "control_parser.rl" #line 158 "control_parser.rl"
g_string_truncate(ctx->tmp, 0); g_string_truncate(ctx->tmp, 0);
ctx->mark = -1; ctx->mark = -1;
/* fprintf(stderr, "Parser reset\n"); */ /* fprintf(stderr, "Parser reset\n"); */
@ -399,13 +399,17 @@ _match:
} else { } else {
/* fprintf(stderr, "time difference [ms]: %i\n", getcurts(t) - ctx->tm->ts); */ /* fprintf(stderr, "time difference [ms]: %i\n", getcurts(t) - ctx->tm->ts); */
} }
/* Reset simulation */
t->sim.ts = ctx->tm->ts;
t->sim.vehicle = t->vehicle;
t->last_ts = ctx->tm->ts; t->last_ts = ctx->tm->ts;
ctx->tm = NULL; ctx->tm = NULL;
t->alive = 1; t->alive = 1;
} }
break; break;
case 4: case 4:
#line 55 "control_parser.rl" #line 59 "control_parser.rl"
{ {
ctx->tm->ts = ctx->ts; ctx->tm->ts = ctx->ts;
ctx->tm->vehicle.x = ctx->x; ctx->tm->vehicle.x = ctx->x;
@ -420,115 +424,115 @@ _match:
} }
break; break;
case 5: case 5:
#line 72 "control_parser.rl" #line 76 "control_parser.rl"
{ t->map.dx = extract_double(ctx, p); } { t->map.dx = extract_double(ctx, p); }
break; break;
case 6: case 6:
#line 73 "control_parser.rl" #line 77 "control_parser.rl"
{ t->map.dy = extract_double(ctx, p); } { t->map.dy = extract_double(ctx, p); }
break; break;
case 7: case 7:
#line 74 "control_parser.rl" #line 78 "control_parser.rl"
{ t->map.min_sensor = extract_double(ctx, p); } { t->map.min_sensor = extract_double(ctx, p); }
break; break;
case 8: case 8:
#line 75 "control_parser.rl" #line 79 "control_parser.rl"
{ t->map.max_sensor = extract_double(ctx, p); } { t->map.max_sensor = extract_double(ctx, p); }
break; break;
case 9: case 9:
#line 76 "control_parser.rl" #line 80 "control_parser.rl"
{ t->map.max_speed = extract_double(ctx, p); } { t->map.max_speed = extract_double(ctx, p); }
break; break;
case 10: case 10:
#line 77 "control_parser.rl" #line 81 "control_parser.rl"
{ t->map.max_turn = extract_double(ctx, p); } { t->map.max_turn = extract_double(ctx, p); }
break; break;
case 11: case 11:
#line 78 "control_parser.rl" #line 82 "control_parser.rl"
{ t->map.max_hard_turn = extract_double(ctx, p); } { t->map.max_hard_turn = extract_double(ctx, p); }
break; break;
case 12: case 12:
#line 82 "control_parser.rl" #line 86 "control_parser.rl"
{ ctx->x = extract_double(ctx, p); } { ctx->x = extract_double(ctx, p); }
break; break;
case 13: case 13:
#line 83 "control_parser.rl" #line 87 "control_parser.rl"
{ ctx->y = extract_double(ctx, p); } { ctx->y = extract_double(ctx, p); }
break; break;
case 14: case 14:
#line 84 "control_parser.rl" #line 88 "control_parser.rl"
{ ctx->r = extract_double(ctx, p); } { ctx->r = extract_double(ctx, p); }
break; break;
case 15: case 15:
#line 85 "control_parser.rl" #line 89 "control_parser.rl"
{ ctx->dir = extract_double(ctx, p); } { ctx->dir = extract_double(ctx, p); }
break; break;
case 16: case 16:
#line 86 "control_parser.rl" #line 90 "control_parser.rl"
{ ctx->speed = extract_double(ctx, p); } { ctx->speed = extract_double(ctx, p); }
break; break;
case 17: case 17:
#line 88 "control_parser.rl" #line 92 "control_parser.rl"
{ ctx->tm->vehicle.accel = ACCEL; } { ctx->tm->vehicle.accel = ACCEL; }
break; break;
case 18: case 18:
#line 89 "control_parser.rl" #line 93 "control_parser.rl"
{ ctx->tm->vehicle.accel = ROLL; } { ctx->tm->vehicle.accel = ROLL; }
break; break;
case 19: case 19:
#line 90 "control_parser.rl" #line 94 "control_parser.rl"
{ ctx->tm->vehicle.accel = BRAKE; } { ctx->tm->vehicle.accel = BRAKE; }
break; break;
case 20: case 20:
#line 92 "control_parser.rl" #line 96 "control_parser.rl"
{ ctx->tm->vehicle.turn = TURN_HARD_LEFT; } { ctx->tm->vehicle.turn = TURN_HARD_LEFT; }
break; break;
case 21: case 21:
#line 93 "control_parser.rl" #line 97 "control_parser.rl"
{ ctx->tm->vehicle.turn = TURN_LEFT; } { ctx->tm->vehicle.turn = TURN_LEFT; }
break; break;
case 22: case 22:
#line 94 "control_parser.rl" #line 98 "control_parser.rl"
{ ctx->tm->vehicle.turn = TURN_STRAIGHT; } { ctx->tm->vehicle.turn = TURN_STRAIGHT; }
break; break;
case 23: case 23:
#line 95 "control_parser.rl" #line 99 "control_parser.rl"
{ ctx->tm->vehicle.turn = TURN_RIGHT; } { ctx->tm->vehicle.turn = TURN_RIGHT; }
break; break;
case 24: case 24:
#line 96 "control_parser.rl" #line 100 "control_parser.rl"
{ ctx->tm->vehicle.turn = TURN_HARD_RIGHT; } { ctx->tm->vehicle.turn = TURN_HARD_RIGHT; }
break; break;
case 25: case 25:
#line 98 "control_parser.rl" #line 102 "control_parser.rl"
{ ctx->ts = extract_ts(ctx, p); } { ctx->ts = extract_ts(ctx, p); }
break; break;
case 26: case 26:
#line 99 "control_parser.rl" #line 103 "control_parser.rl"
{ printf("Score %u\n", extract_ts(ctx, p)); } { printf("Score %u\n", extract_ts(ctx, p)); }
break; break;
case 27: case 27:
#line 101 "control_parser.rl" #line 105 "control_parser.rl"
{ {
object o = { BOLDER, ctx->x, ctx->y, ctx->r, 0, 0 }; object o = { BOLDER, ctx->x, ctx->y, ctx->r, 0, 0 };
g_array_append_val(ctx->tm->objects, o); g_array_append_val(ctx->tm->objects, o);
} }
break; break;
case 28: case 28:
#line 105 "control_parser.rl" #line 109 "control_parser.rl"
{ {
object o = { CRATER, ctx->x, ctx->y, ctx->r, 0, 0 }; object o = { CRATER, ctx->x, ctx->y, ctx->r, 0, 0 };
g_array_append_val(ctx->tm->objects, o); g_array_append_val(ctx->tm->objects, o);
} }
break; break;
case 29: case 29:
#line 110 "control_parser.rl" #line 114 "control_parser.rl"
{ {
object o = { MARTIAN, ctx->x, ctx->y, ctx->r, ctx->dir, ctx->speed }; object o = { MARTIAN, ctx->x, ctx->y, MARTIAN_RADIUS, ctx->dir, ctx->speed };
g_array_append_val(ctx->tm->objects, o); g_array_append_val(ctx->tm->objects, o);
} }
break; break;
#line 532 "control_parser.c" #line 536 "control_parser.c"
} }
} }
@ -540,7 +544,7 @@ _again:
_test_eof: {} _test_eof: {}
_out: {} _out: {}
} }
#line 177 "control_parser.rl" #line 181 "control_parser.rl"
ctx->pos = p - ctx->buffer->str; ctx->pos = p - ctx->buffer->str;
if (ctx->mark == -1) { if (ctx->mark == -1) {

View File

@ -48,6 +48,10 @@ static timestamp extract_ts(context *ctx, char *fpc) {
} else { } else {
/* fprintf(stderr, "time difference [ms]: %i\n", getcurts(t) - ctx->tm->ts); */ /* fprintf(stderr, "time difference [ms]: %i\n", getcurts(t) - ctx->tm->ts); */
} }
/* Reset simulation */
t->sim.ts = ctx->tm->ts;
t->sim.vehicle = t->vehicle;
t->last_ts = ctx->tm->ts; t->last_ts = ctx->tm->ts;
ctx->tm = NULL; ctx->tm = NULL;
t->alive = 1; t->alive = 1;

View File

@ -4,8 +4,6 @@
#include <stdio.h> #include <stdio.h>
void godown(trial *t);
void trial_loop(trial *t) { void trial_loop(trial *t) {
// path *p; // path *p;
// int offset; // int offset;
@ -18,8 +16,9 @@ void trial_loop(trial *t) {
// offset = path_app_target(p,t,offset,0,0); // offset = path_app_target(p,t,offset,0,0);
while (t->alive) { while (t->alive) {
// path_execute(t,p); // path_execute(t,p);
simulate(t);
godown(t); godown(t);
if (-1 == trial_wait_for_input(t)) return; if (-1 == trial_check_input(t)) return;
} }
trial_matlab(t); trial_matlab(t);
trial_reset_run(t); trial_reset_run(t);

View File

@ -20,14 +20,14 @@ void godown(trial *t) {
double xpot, ypot, r, angle; double xpot, ypot, r, angle;
/* todo: simulate movement */ /* todo: simulate movement */
v = t->vehicle; v = t->sim.vehicle;
tm = (telemetry*) g_queue_peek_tail(&t->telemetry); tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
r = sqrt(v.x*v.x + v.y*v.y); r = sqrt(v.x*v.x + v.y*v.y);
if (r < 4) return; /* done */ if (r < 4) return; /* done */
xpot = v.x/r; xpot = 0.2 * v.x/r;
ypot = v.y/r; ypot = 0.2 * v.y/r;
for (guint i = 0; i < tm->objects->len; i++) { for (guint i = 0; i < tm->objects->len; i++) {
object *o = &g_array_index(tm->objects, object, i); object *o = &g_array_index(tm->objects, object, i);
@ -36,20 +36,21 @@ void godown(trial *t) {
switch (o->type) { switch (o->type) {
case BOLDER: case BOLDER:
sigma = 2 * o->rad; sigma = 2;
h = 1; h = 1;
break; break;
case CRATER: case CRATER:
sigma = 2 * o->rad; sigma = 4;
h = 3; h = 1;
break; break;
case MARTIAN: case MARTIAN:
sigma = 4 * o->rad; sigma = 8;
h = 1; h = 1;
break; break;
} }
r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) ); r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) ) - o->rad;
// r = r*r*r * 10;
r = C1 * h / (sigma*sigma) / exp (r / (2*sigma) ); r = C1 * h / (sigma*sigma) / exp (r / (2*sigma) );
xpot += (o->x - v.x) * r; xpot += (o->x - v.x) * r;
ypot += (o->y - v.y) * r; ypot += (o->y - v.y) * r;
@ -70,10 +71,11 @@ void godown(trial *t) {
vehicle_left(t); vehicle_left(t);
} }
} }
fprintf(stderr, "Angle: %f, objects: %u\n", angle, tm->objects->len);
angle = abs(angle); angle = abs(angle);
if (angle > 60) { if (angle > 60) {
vehicle_break(t); vehicle_break(t);
} else if (angle > 30) { } else if (angle > 50) {
vehicle_roll(t); vehicle_roll(t);
} else { } else {
vehicle_accel(t); vehicle_accel(t);

35
src/simulate.c Normal file
View File

@ -0,0 +1,35 @@
#include "control.h"
#include <math.h>
#define LATENCY 20
static double fixdir(double a) {
if (a < 0) a += 360;
else if (a > 360) a -= 360;
return a;
}
void simulate(trial *t) {
double move, angle;
timestamp now = getcurts(t) + LATENCY, diff = now - t->sim.ts;
if (diff < 20) return;
/* move = t->sim.vehicle.speed / 1000 * diff;
angle = t->sim.vehicle.dir*M_PI/180;
t->sim.vehicle.x += cos(angle)*move;
t->sim.vehicle.y += sin(angle)*move;*/
/* switch (t->sim.vehicle.turn) {
case TURN_HARD_LEFT : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir + t->map.max_hard_turn); break;
case TURN_LEFT : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir + t->map.max_turn); break;
case TURN_STRAIGHT : break;
case TURN_RIGHT : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir - t->map.max_turn); break;
case TURN_HARD_RIGHT: t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir - t->map.max_hard_turn); break;
}
t->sim.vehicle.accel = t->vehicle.accel;
t->sim.vehicle.turn = t->vehicle.turn;*/
}

View File

@ -9,6 +9,7 @@ main_source = '''
control_parser.c control_parser.c
path.c path.c
potential.c potential.c
simulate.c
''' '''
def build(bld): def build(bld):