From 12d711e9c2054d252f5de296d14ef92cdf5a58e7 Mon Sep 17 00:00:00 2001
From: Johannes Reinhardt <jreinhardt@jr-thinkpad.(none)>
Date: Sat, 12 Jul 2008 12:21:49 +0200
Subject: [PATCH] major refactoring of path

---
 src/main.c |  7 ++++--
 src/path.c | 74 +++++++++++++++++++++++++++++++++++-------------------
 src/path.h |  4 ++-
 3 files changed, 56 insertions(+), 29 deletions(-)

diff --git a/src/main.c b/src/main.c
index 18d484d..bec4d33 100644
--- a/src/main.c
+++ b/src/main.c
@@ -6,15 +6,19 @@
 
 void trial_loop(trial *t) {
 	path *p;
+	int offset;
 	do {
 		if (-1 == trial_wait_for_start(t)) return;
 		if (t->finished) break;
+		p = path_new();
 		/*create trivial path towards the origin*/
-		p = path_new(&t->map, &t->vehicle);
+		offset = path_app_fitseq(p,t,0);
+		offset = path_app_target(p,t,offset,0,0);
 		while (t->alive) {
 			path_execute(t,p);
 			if (-1 == trial_check_input(t)) return;
 		}
+		trial_matlab(t);
 		trial_reset_run(t);
 		path_free(p);
 	} while (!t->finished);
@@ -33,7 +37,6 @@ int main(int argc, char **argv) {
 	}
 
 	trial_loop(t);
-	trial_matlab(t);
 	
 	trial_free(t);
 	
diff --git a/src/path.c b/src/path.c
index 54663ce..04a5263 100644
--- a/src/path.c
+++ b/src/path.c
@@ -23,35 +23,11 @@ double angle_for_rot(double from, double to) {
 	return a;
 }
 
-path *path_new(map* m,vehicle *v){
-	command* tmp;
+path *path_new(){
 	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 = atan2(-v->y,-v->x)*180/M_PI;
-	angle = angle_for_rot(v->dir, angle);
-	stop = abs(angle)/m->max_hard_turn*1000;
-	printf("Angle: %f stop: %f\n",angle,stop);
-	if(angle > 0){
-		/*clockwise/left turn*/
-		tmp = command_new(0,BREAK,TURN_HARD_LEFT);
-		g_queue_push_tail(res->commands,tmp);
-	} else {
-		/*counterclockwise/right turn*/
-		tmp = command_new(0,BREAK,TURN_HARD_RIGHT);
-		g_queue_push_tail(res->commands,tmp);
-	}
-	tmp = command_new(stop,ACCEL,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;
 }
 
@@ -82,7 +58,7 @@ void path_execute(trial* t,path* p){
 		switch(tmp->accel){
 			case ACCEL: vehicle_accel(t); break;
 			case ROLL: vehicle_roll(t); break;
-			case BREAK: vehicle_break(t); break;
+			case BRAKE: vehicle_break(t); break;
 		}
 		command_free(tmp);
 		tmp = (command*) g_queue_peek_head(p->commands);
@@ -107,3 +83,49 @@ void path_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 = abs(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){
+	command* tmp;
+	int testtime = 300;
+	tmp = command_new(offset,ACCEL,TURN_STRAIGHT);
+	g_queue_push_tail(p->commands,tmp);
+	tmp = command_new(offset+testtime,ROLL,TURN_STRAIGHT);
+	g_queue_push_tail(p->commands,tmp);
+	tmp = command_new(offset+2*testtime,BRAKE,TURN_STRAIGHT);
+	g_queue_push_tail(p->commands,tmp);
+	return 3*testtime;
+}
+	
+	
+
diff --git a/src/path.h b/src/path.h
index 8ef5b72..cae5761 100644
--- a/src/path.h
+++ b/src/path.h
@@ -24,9 +24,11 @@ command *command_new(timestamp ts, accel_t a, turn_t t);
 void command_free(command* c);
 
 
-path *path_new(map* m,vehicle *c);
+path *path_new();
 void path_execute(trial* t,path* p);
 void path_free(path* p);
 
 
+int path_app_fitseq(path* p, trial* t, timestamp offset);
+int path_app_target(path* p, trial* t, int offset, int x, int y);
 #endif