102 lines
2.5 KiB
C
102 lines
2.5 KiB
C
#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);
|
|
}
|
|
|
|
path *path_new(map* m,vehicle *v){
|
|
command* tmp;
|
|
path* res;
|
|
double angle, stop;
|
|
|
|
res = g_slice_new(path);
|
|
res->commands = g_queue_new();
|
|
/* Calculate a trivial path to the origin*/
|
|
|
|
/* Turn towards origin, take shorter direction*/
|
|
angle = atan(v->y/v->x)*180/M_PI;
|
|
stop = abs(angle - v->dir)/m->max_turn*1000;
|
|
printf("Angle: %f stop: %f\n",angle,stop);
|
|
if(angle - v->dir > 0){
|
|
/*clockwise/left turn*/
|
|
tmp = command_new(0,BREAK,TURN_LEFT);
|
|
g_queue_push_tail(res->commands,tmp);
|
|
} else {
|
|
/*counterclockwise/right turn*/
|
|
tmp = command_new(0,BREAK,TURN_RIGHT);
|
|
g_queue_push_tail(res->commands,tmp);
|
|
}
|
|
tmp = command_new(stop,BREAK,TURN_STRAIGHT);
|
|
g_queue_push_tail(res->commands,tmp);
|
|
|
|
/*start driving*/
|
|
tmp = command_new(stop,ACCEL,TURN_STRAIGHT);
|
|
g_queue_push_tail(res->commands,tmp);
|
|
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 = timestamp_get_curts(t);
|
|
/*fprintf(stderr,"Now: %d TS: %d\n",now,clock());*/
|
|
/*magic number for latency, send messages that much earlier*/
|
|
while(tmp != NULL && now - 20 > tmp->ts){
|
|
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);
|
|
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);
|
|
}
|
|
|
|
|