icfp11/src/path.c

97 lines
2.3 KiB
C
Raw Normal View History

2008-07-12 00:25:19 +00:00
#include <stdio.h>
#include <stdlib.h>
2008-07-11 23:40:57 +00:00
#include <math.h>
#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;
2008-07-12 00:25:19 +00:00
return res;
2008-07-11 23:40:57 +00:00
}
void command_free(command* c){
g_slice_free(command, c);
}
path *path_new(map* m,vehicle *v){
command* tmp;
path* res;
double angle, stop;
2008-07-12 00:37:53 +00:00
res = g_slice_new(path);
2008-07-11 23:40:57 +00:00
res->commands = g_queue_new();
/* Calculate a trivial path to the origin*/
/* Turn towards origin, take shorter direction*/
2008-07-12 00:25:19 +00:00
angle = atan(v->y/v->x);
2008-07-11 23:40:57 +00:00
stop = abs(angle - v->dir)/m->max_turn;
if(angle - v->dir > 0){
/*clockwise/left turn*/
tmp = command_new(0,BREAK,TURN_LEFT);
2008-07-12 00:25:19 +00:00
g_queue_push_tail(res->commands,tmp);
} else {
2008-07-11 23:40:57 +00:00
/*counterclockwise/right turn*/
tmp = command_new(0,BREAK,TURN_RIGHT);
2008-07-12 00:25:19 +00:00
g_queue_push_tail(res->commands,tmp);
2008-07-11 23:40:57 +00:00
}
2008-07-12 01:47:36 +00:00
tmp = command_new(stop,BREAK,TURN_STRAIGHT);
g_queue_push_tail(res->commands,tmp);
2008-07-11 23:40:57 +00:00
/*start driving*/
tmp = command_new(stop,ACCEL,TURN_STRAIGHT);
2008-07-12 00:25:19 +00:00
g_queue_push_tail(res->commands,tmp);
return res;
2008-07-11 23:40:57 +00:00
}
void path_execute(trial* t,path* p){
command* tmp;
tmp = (command*) g_queue_peek_head(p->commands);
2008-07-12 01:47:36 +00:00
if(tmp == NULL){
fprintf(stderr,"warning: cannot execute empty path\n");
}
if(t == NULL){
fprintf(stderr,"trial is null\n");
return;
}
2008-07-11 23:40:57 +00:00
/*magic number for latency, send messages that much earlier*/
2008-07-12 01:47:36 +00:00
while(tmp != NULL && tmp->ts - t->last_ts < 20){
2008-07-11 23:40:57 +00:00
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");
2008-07-12 00:25:19 +00:00
while(g_queue_get_length(p->commands) > 0){
tmp = g_queue_pop_head(p->commands);
fprintf(stderr,"\tTimestamp: %d Turn: %d Accel: %d\n",tmp->ts,tmp->turn,tmp->accel);
2008-07-11 23:40:57 +00:00
command_free(tmp);
}
}
g_queue_free(p->commands);
g_slice_free(path,p);
}