added untested source files for paths

src/path.c

@@ -0,0 +1,85 @@
+
+#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;
+}
+
+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);
+}
+
+

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