From ec507f4ba85ec5bf41a7a6e489bafad282708b93 Mon Sep 17 00:00:00 2001 From: Johannes Reinhardt Date: Sat, 12 Jul 2008 01:40:57 +0200 Subject: [PATCH 1/2] added untested source files for paths --- src/path.c | 85 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/path.h | 27 +++++++++++++++++ 2 files changed, 112 insertions(+) create mode 100644 src/path.c create mode 100644 src/path.h diff --git a/src/path.c b/src/path.c new file mode 100644 index 0000000..cd33a08 --- /dev/null +++ b/src/path.c @@ -0,0 +1,85 @@ + +#include +#include "path.h" + +command *command_new(timestamp ts, accel_t a, turn_t t){ + command* res; + res = g_slice_new(command); + res->ts = ts; + res->accel = a; + res->turn = t; +} + +void command_free(command* c){ + g_slice_free(command, c); +} + +path *path_new(map* m,vehicle *v){ + command* tmp; + path* res; + double angle, stop; + res->commands = g_queue_new(); + /* Calculate a trivial path to the origin*/ + + /* Turn towards origin, take shorter direction*/ + angle = atan(v->y/y->x); + stop = abs(angle - v->dir)/m->max_turn; + if(angle - v->dir > 0){ + /*clockwise/left turn*/ + tmp = command_new(0,BREAK,TURN_LEFT); + g_push_tail(res-commands,tmp); + tmp = command_new(stop,BREAK,TURN_RIGHT); + g_push_tail(res-commands,tmp); + else { + /*counterclockwise/right turn*/ + tmp = command_new(0,BREAK,TURN_RIGHT); + g_push_tail(res-commands,tmp); + tmp = command_new(stop,BREAK,TURN_LEFT); + g_push_tail(res-commands,tmp); + } + + /*start driving*/ + tmp = command_new(stop,ACCEL,TURN_STRAIGHT); + g_push_tail(res-commands,tmp); +} + +void path_execute(trial* t,path* p){ + command* tmp; + tmp = (command*) g_queue_peek_head(p->commands); + /*magic number for latency, send messages that much earlier*/ + while(tmp->ts - t->last_ts < 20){ + tmp = (command*) g_queue_pop_head(p->commands); + switch(tmp->turn){ + case TURN_HARD_LEFT: vehicle_hard_left(t); break; + case TURN_LEFT: vehicle_left(t); break; + case TURN_STRAIGHT: vehicle_straight(t); break; + case TURN_RIGHT: vehicle_right(t); break; + case TURN_HARD_RIGHT: vehicle_hard_right(t); break; + } + switch(tmp->accel){ + case ACCEL: vehicle_accel(t); break; + case ROLL: vehicle_roll(t); break; + case BREAK: vehicle_break(t); break; + } + command_free(tmp); + tmp = (command*) g_queue_peek_head(p->commands); + } +} + + +void path_free(path* p){ + int length; + command* tmp; + length = g_queue_get_length(p->commands); + if(length > 0){ + fprintf(stderr,"freeing non-empty path\n"); + while(g_queue_get_length(p->commands > 0)){ + tmp = g_queue_pop_head(p->commands); + command_free(tmp); + } + } + g_queue_free(p->commands); + g_slice_free(path,p); +} + + diff --git a/src/path.h b/src/path.h new file mode 100644 index 0000000..f0168d1 --- /dev/null +++ b/src/path.h @@ -0,0 +1,27 @@ +#ifndef _PATH_H +#define _PATH_H + + +struct path; +typedef struct path path; + +struct command{ + timestamp ts; + accel_t accel; + turn_t turn; +} + +struct path { + GQueue* commands; +} + +command *command_new(timestamp ts, accel_t a, turn_t t); +void command_free(command* c); + + +path *path_new(map* m,vehicle *c); +void path_execute(trial* t,path* p); +void path_free(path* p); + + +#endif From 6ec6d6015bc590249dfb96a9d8b574522726bddc Mon Sep 17 00:00:00 2001 From: Johannes Reinhardt Date: Sat, 12 Jul 2008 02:00:11 +0200 Subject: [PATCH 2/2] added a first implementation to main fixed compilation (apart from trial->alive) --- src/main.c | 11 +++++++++++ src/path.h | 9 +++++++-- src/wscript | 1 + 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main.c b/src/main.c index 320eee1..1d48cfd 100644 --- a/src/main.c +++ b/src/main.c @@ -1,10 +1,12 @@ #include "control.h" +#include "path.h" #include int main(int argc, char **argv) { trial *t; + path *p; if (argc <= 2) { fprintf(stderr, "Syntax: %s hostname port\n", argv[0]); @@ -14,7 +16,16 @@ int main(int argc, char **argv) { if (NULL == (t = trial_new(argv[1], argv[2]))) { return 2; } + + /*create trivial path towards the origin*/ + p = path_new(&t->map,&t->vehicle); + while(t->alive){ + trial_wait_for_input(t); + path_execute(t,p); + } + + path_free(p); trial_free(t); return 0; diff --git a/src/path.h b/src/path.h index f0168d1..8ef5b72 100644 --- a/src/path.h +++ b/src/path.h @@ -1,19 +1,24 @@ #ifndef _PATH_H #define _PATH_H +#include "control.h" + struct path; typedef struct path path; +struct command; +typedef struct command command; + struct command{ timestamp ts; accel_t accel; turn_t turn; -} +}; struct path { GQueue* commands; -} +}; command *command_new(timestamp ts, accel_t a, turn_t t); void command_free(command* c); diff --git a/src/wscript b/src/wscript index 0f2bcbc..8da3453 100644 --- a/src/wscript +++ b/src/wscript @@ -6,6 +6,7 @@ main_source = ''' main.c control.c + path.c ''' def build(bld):