From d37d09ee669ecf6862ae35051b55b0e33dae0f5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BChler?= Date: Sat, 12 Jul 2008 17:50:24 +0200 Subject: [PATCH] Simulate --- src/control.h | 23 ++++++++++++-- src/control_parser.c | 70 +++++++++++++++++++++++-------------------- src/control_parser.rl | 4 +++ src/main.c | 5 ++-- src/potential.c | 20 +++++++------ src/simulate.c | 35 ++++++++++++++++++++++ src/wscript | 1 + 7 files changed, 110 insertions(+), 48 deletions(-) create mode 100644 src/simulate.c diff --git a/src/control.h b/src/control.h index 2517f30..f25b85a 100644 --- a/src/control.h +++ b/src/control.h @@ -31,6 +31,9 @@ typedef struct vehicle vehicle; struct telemetry; typedef struct telemetry telemetry; +struct simulated; +typedef struct simulated simulated; + struct map; typedef struct map map; @@ -69,19 +72,30 @@ struct map { GArray *solid_objects; }; +struct simulated { + timestamp ts; + vehicle vehicle; +}; + struct trial { map map; timestamp last_ts; GQueue telemetry; - vehicle vehicle; /* our view */ + vehicle vehicle; int alive, finished; - struct timeval started; + + simulated sim; /* internal */ + struct timeval started; int socket; control_parser_ctx *parse_ctx; }; +/* extern */ +void simulate(trial *t); +void godown(trial *t); + /* trial */ trial *trial_new(const char *hostname, const char *port); void trial_reset_run(trial *t); @@ -116,7 +130,10 @@ telemetry* telemetry_new(); /* 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) { switch (t->vehicle.accel) { diff --git a/src/control_parser.c b/src/control_parser.c index 5990386..cc210a3 100644 --- a/src/control_parser.c +++ b/src/control_parser.c @@ -27,7 +27,7 @@ static timestamp extract_ts(context *ctx, char *fpc) { 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; -#line 133 "control_parser.rl" +#line 137 "control_parser.rl" static int control_parser_has_error(context *ctx) { return ctx->cs == control_parser_error; @@ -260,7 +260,7 @@ void control_parser_new(trial *t) { { ( ctx->cs) = control_parser_start; } -#line 145 "control_parser.rl" +#line 149 "control_parser.rl" ctx->buffer = g_string_sized_new(0); ctx->tmp = g_string_sized_new(0); ctx->mark = -1; @@ -274,7 +274,7 @@ void control_parser_reset(trial *t) { { ( ctx->cs) = control_parser_start; } -#line 154 "control_parser.rl" +#line 158 "control_parser.rl" g_string_truncate(ctx->tmp, 0); ctx->mark = -1; /* fprintf(stderr, "Parser reset\n"); */ @@ -399,13 +399,17 @@ _match: } else { /* 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; ctx->tm = NULL; t->alive = 1; } break; case 4: -#line 55 "control_parser.rl" +#line 59 "control_parser.rl" { ctx->tm->ts = ctx->ts; ctx->tm->vehicle.x = ctx->x; @@ -420,115 +424,115 @@ _match: } break; case 5: -#line 72 "control_parser.rl" +#line 76 "control_parser.rl" { t->map.dx = extract_double(ctx, p); } break; case 6: -#line 73 "control_parser.rl" +#line 77 "control_parser.rl" { t->map.dy = extract_double(ctx, p); } break; case 7: -#line 74 "control_parser.rl" +#line 78 "control_parser.rl" { t->map.min_sensor = extract_double(ctx, p); } break; case 8: -#line 75 "control_parser.rl" +#line 79 "control_parser.rl" { t->map.max_sensor = extract_double(ctx, p); } break; case 9: -#line 76 "control_parser.rl" +#line 80 "control_parser.rl" { t->map.max_speed = extract_double(ctx, p); } break; case 10: -#line 77 "control_parser.rl" +#line 81 "control_parser.rl" { t->map.max_turn = extract_double(ctx, p); } break; case 11: -#line 78 "control_parser.rl" +#line 82 "control_parser.rl" { t->map.max_hard_turn = extract_double(ctx, p); } break; case 12: -#line 82 "control_parser.rl" +#line 86 "control_parser.rl" { ctx->x = extract_double(ctx, p); } break; case 13: -#line 83 "control_parser.rl" +#line 87 "control_parser.rl" { ctx->y = extract_double(ctx, p); } break; case 14: -#line 84 "control_parser.rl" +#line 88 "control_parser.rl" { ctx->r = extract_double(ctx, p); } break; case 15: -#line 85 "control_parser.rl" +#line 89 "control_parser.rl" { ctx->dir = extract_double(ctx, p); } break; case 16: -#line 86 "control_parser.rl" +#line 90 "control_parser.rl" { ctx->speed = extract_double(ctx, p); } break; case 17: -#line 88 "control_parser.rl" +#line 92 "control_parser.rl" { ctx->tm->vehicle.accel = ACCEL; } break; case 18: -#line 89 "control_parser.rl" +#line 93 "control_parser.rl" { ctx->tm->vehicle.accel = ROLL; } break; case 19: -#line 90 "control_parser.rl" +#line 94 "control_parser.rl" { ctx->tm->vehicle.accel = BRAKE; } break; case 20: -#line 92 "control_parser.rl" +#line 96 "control_parser.rl" { ctx->tm->vehicle.turn = TURN_HARD_LEFT; } break; case 21: -#line 93 "control_parser.rl" +#line 97 "control_parser.rl" { ctx->tm->vehicle.turn = TURN_LEFT; } break; case 22: -#line 94 "control_parser.rl" +#line 98 "control_parser.rl" { ctx->tm->vehicle.turn = TURN_STRAIGHT; } break; case 23: -#line 95 "control_parser.rl" +#line 99 "control_parser.rl" { ctx->tm->vehicle.turn = TURN_RIGHT; } break; case 24: -#line 96 "control_parser.rl" +#line 100 "control_parser.rl" { ctx->tm->vehicle.turn = TURN_HARD_RIGHT; } break; case 25: -#line 98 "control_parser.rl" +#line 102 "control_parser.rl" { ctx->ts = extract_ts(ctx, p); } break; case 26: -#line 99 "control_parser.rl" +#line 103 "control_parser.rl" { printf("Score %u\n", extract_ts(ctx, p)); } break; case 27: -#line 101 "control_parser.rl" +#line 105 "control_parser.rl" { object o = { BOLDER, ctx->x, ctx->y, ctx->r, 0, 0 }; g_array_append_val(ctx->tm->objects, o); } break; case 28: -#line 105 "control_parser.rl" +#line 109 "control_parser.rl" { object o = { CRATER, ctx->x, ctx->y, ctx->r, 0, 0 }; g_array_append_val(ctx->tm->objects, o); } break; 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); } break; -#line 532 "control_parser.c" +#line 536 "control_parser.c" } } @@ -540,7 +544,7 @@ _again: _test_eof: {} _out: {} } -#line 177 "control_parser.rl" +#line 181 "control_parser.rl" ctx->pos = p - ctx->buffer->str; if (ctx->mark == -1) { diff --git a/src/control_parser.rl b/src/control_parser.rl index 71a0644..25093a1 100644 --- a/src/control_parser.rl +++ b/src/control_parser.rl @@ -48,6 +48,10 @@ static timestamp extract_ts(context *ctx, char *fpc) { } else { /* 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; ctx->tm = NULL; t->alive = 1; diff --git a/src/main.c b/src/main.c index 64c5731..2b0b8b4 100644 --- a/src/main.c +++ b/src/main.c @@ -4,8 +4,6 @@ #include -void godown(trial *t); - void trial_loop(trial *t) { // path *p; // int offset; @@ -18,8 +16,9 @@ void trial_loop(trial *t) { // offset = path_app_target(p,t,offset,0,0); while (t->alive) { // path_execute(t,p); + simulate(t); godown(t); - if (-1 == trial_wait_for_input(t)) return; + if (-1 == trial_check_input(t)) return; } trial_matlab(t); trial_reset_run(t); diff --git a/src/potential.c b/src/potential.c index 1b3350d..0d20e6a 100644 --- a/src/potential.c +++ b/src/potential.c @@ -20,14 +20,14 @@ void godown(trial *t) { double xpot, ypot, r, angle; /* todo: simulate movement */ - v = t->vehicle; + v = t->sim.vehicle; tm = (telemetry*) g_queue_peek_tail(&t->telemetry); r = sqrt(v.x*v.x + v.y*v.y); if (r < 4) return; /* done */ - xpot = v.x/r; - ypot = v.y/r; + xpot = 0.2 * v.x/r; + ypot = 0.2 * v.y/r; for (guint i = 0; i < tm->objects->len; i++) { object *o = &g_array_index(tm->objects, object, i); @@ -36,20 +36,21 @@ void godown(trial *t) { switch (o->type) { case BOLDER: - sigma = 2 * o->rad; + sigma = 2; h = 1; break; case CRATER: - sigma = 2 * o->rad; - h = 3; + sigma = 4; + h = 1; break; case MARTIAN: - sigma = 4 * o->rad; + sigma = 8; h = 1; 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) ); xpot += (o->x - v.x) * r; ypot += (o->y - v.y) * r; @@ -70,10 +71,11 @@ void godown(trial *t) { vehicle_left(t); } } + fprintf(stderr, "Angle: %f, objects: %u\n", angle, tm->objects->len); angle = abs(angle); if (angle > 60) { vehicle_break(t); - } else if (angle > 30) { + } else if (angle > 50) { vehicle_roll(t); } else { vehicle_accel(t); diff --git a/src/simulate.c b/src/simulate.c new file mode 100644 index 0000000..1d63b6e --- /dev/null +++ b/src/simulate.c @@ -0,0 +1,35 @@ + +#include "control.h" + +#include + +#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;*/ +} diff --git a/src/wscript b/src/wscript index 1e88a97..f5b758f 100644 --- a/src/wscript +++ b/src/wscript @@ -9,6 +9,7 @@ main_source = ''' control_parser.c path.c potential.c + simulate.c ''' def build(bld):