You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
icfp11/src/path.c

176 lines
4.2 KiB

#include <stdio.h>
#include <stdlib.h>
#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;
return res;
}
void command_free(command* c){
g_slice_free(command, c);
}
static double angle_for_rot(double from, double to) {
double a = to - from;
if (a > 180) a = -360 + a;
else if (a < -180) a = 360 + a;
return a;
}
path *path_new(){
path* res;
res = g_slice_new(path);
res->commands = g_queue_new();
return res;
}
void path_execute(trial* t,path* p){
command* tmp;
timestamp now;
tmp = (command*) g_queue_peek_head(p->commands);
/*if(tmp == NULL){
fprintf(stderr,"warning: cannot execute empty path\n");
}*/
if(t == NULL){
fprintf(stderr,"trial is null\n");
return;
}
now = getcurts(t);
while(tmp != NULL && now > tmp->ts ){
fprintf(stderr, "now: %u, ts: %u, turn: %i, accel: %i\n", now, tmp->ts, tmp->turn, tmp->accel);
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 BRAKE: 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);
fprintf(stderr,"\tTimestamp: %d Turn: %d Accel: %d\n",tmp->ts,tmp->turn,tmp->accel);
command_free(tmp);
}
}
g_queue_free(p->commands);
g_slice_free(path,p);
}
/*common path building bricks, to be called in rest*/
int path_app_target(path* p, trial* t,int offset, int x, int y){
command* tmp;
double angle, stop;
vehicle* v;
map* m;
v = &t->vehicle;
m = &t->map;
/* Turn towards origin, take shorter direction, TODO: fix, thats only an approximation*/
angle = atan2(y-v->y,x-v->x)*180/M_PI;
angle = angle_for_rot(v->dir, angle);
stop = fabs(angle)/m->max_hard_turn*1000;
printf("Angle: %f stop: %f\n",angle,stop);
if(angle > 0){
/*clockwise/left turn*/
tmp = command_new(offset,BRAKE,TURN_HARD_LEFT);
g_queue_push_tail(p->commands,tmp);
} else {
/*counterclockwise/right turn*/
tmp = command_new(offset,BRAKE,TURN_HARD_RIGHT);
g_queue_push_tail(p->commands,tmp);
}
/*start driving*/
tmp = command_new(offset + stop,ACCEL,TURN_STRAIGHT);
g_queue_push_tail(p->commands,tmp);
return stop;
}
int path_app_fitseq(path* p, trial* t,timestamp offset, timestamp testtime){
command* tmp;
if(testtime < 200)
fprintf(stderr,"very short testtime, may yield inaccurate results\n");
tmp = command_new(offset,ACCEL,TURN_STRAIGHT);
g_queue_push_tail(p->commands,tmp);
tmp = command_new(offset+testtime,BRAKE,TURN_STRAIGHT);
g_queue_push_tail(p->commands,tmp);
tmp = command_new(offset+2*testtime,BRAKE,TURN_RIGHT);
g_queue_push_tail(p->commands,tmp);
tmp = command_new(offset+3*testtime,BRAKE,TURN_STRAIGHT);
g_queue_push_tail(p->commands,tmp);
return 4*testtime;
}
/*analyze a fit-sequence at specified offset and testtime*/
void trial_fit(trial* t, int offset, int testtime){
telemetry* tmp;
int len,i;
double v1,v2,t1,t2;
double a, k;
len = g_queue_get_length(&t->telemetry);
/*Linear speed*/
i = 0;
tmp = g_queue_peek_nth(&t->telemetry,i);
while(tmp->ts < offset){
v1 = tmp->vehicle.speed;
t1 = tmp->ts;
i++;
tmp = g_queue_peek_nth(&t->telemetry,i);
}
while(tmp->ts < offset + testtime){
v2 = tmp->vehicle.speed;
t2 = tmp->ts;
i++;
tmp = g_queue_peek_nth(&t->telemetry,i);
}
a = (v2 - v1)/(t2 - t1);
k = a/t->map.max_speed/t->map.max_speed;
/*Rotation*/
while(tmp->ts < offset){
v1 = tmp->vehicle.dir;
t1 = tmp->ts;
i++;
tmp = g_queue_peek_nth(&t->telemetry,i);
}
while(tmp->ts < offset + testtime){
v2 = tmp->vehicle.dir;
t2 = tmp->ts;
i++;
tmp = g_queue_peek_nth(&t->telemetry,i);
}
fprintf(stderr,"a: %f. k: %f\n",a,k);
}