refactored path a little bit

added a function to find vehicle parameters
added a runcounter to trial

maps/small-scatter.wrld

@@ -3,7 +3,7 @@
     "timeLimit" : 30000,
     "vehicleParams" : {
         "maxSpeed" : 20,
-        "accel" : 2,
+        "accel" : 4,
         "brake" : 3,
         "turn" : 20,
         "hardTurn" : 60,

src/control.c

@@ -86,6 +86,7 @@ trial *trial_new(const char *hostname, const char *port) {
 	g_queue_init(&t->telemetry);
 	t->map.solid_objects = g_array_new(FALSE, FALSE, sizeof(object));
 	control_parser_new(t);
+	t->runcnt = 0;
 	return t;
 }
 
@@ -93,6 +94,7 @@ void trial_reset_run(trial *t) {
 	g_queue_foreach(&t->telemetry, telemetry_free, NULL);
 	g_queue_clear(&t->telemetry);
 	t->last_ts = 0;
+	t->runcnt++;
 	control_parser_reset(t);
 }
 
@@ -188,8 +190,11 @@ void telemetry_matlab(gpointer tele, gpointer file){
 /*prints coordinate history to matlab compatible file*/
 void trial_matlab(trial *t){
 	FILE* outfile;
+	char name[80];
+
+	sprintf(name,"velocity%d.m",t->runcnt);
 	
-	outfile = fopen("velocity.m","w");
+	outfile = fopen(name,"w");
 	fprintf(outfile,"v = [];\n");
 	g_queue_foreach(&t->telemetry,telemetry_matlab,outfile);
 	fclose(outfile);

src/control.h

@@ -72,6 +72,7 @@ struct trial {
 	vehicle vehicle; /* our view */
 	int alive, finished;
 	struct timeval started;
+	int runcnt;
 	
 	/* internal */
 	int socket;

src/main.c

@@ -12,13 +12,14 @@ void trial_loop(trial *t) {
 		if (t->finished) break;
 		p = path_new();
 		/*create trivial path towards the origin*/
-		offset = path_app_fitseq(p,t,0);
+		offset = path_app_fitseq(p,t,0,400);
 		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_fit(t,0,400);
 		trial_reset_run(t);
 		path_free(p);
 	} while (!t->finished);

src/path.c

@@ -44,8 +44,7 @@ void path_execute(trial* t,path* p){
 	}
 
 	now = getcurts(t);
-	/*magic number for latency, send messages that much earlier*/
-	while(tmp != NULL && now > tmp->ts + 20){
+	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){
@@ -115,17 +114,63 @@ int path_app_target(path* p, trial* t,int offset, int x, int y){
 	return stop;
 }
 
-int path_app_fitseq(path* p, trial* t,timestamp offset){
+int path_app_fitseq(path* p, trial* t,timestamp offset, timestamp testtime){
 	command* tmp;
-	int testtime = 300;
+	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,ROLL,TURN_STRAIGHT);
+	tmp = command_new(offset+testtime,BRAKE,TURN_STRAIGHT);
 	g_queue_push_tail(p->commands,tmp);
-	tmp = command_new(offset+2*testtime,BRAKE,TURN_STRAIGHT);
+	tmp = command_new(offset+2*testtime,BRAKE,TURN_RIGHT);
 	g_queue_push_tail(p->commands,tmp);
-	return 3*testtime;
+	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);
+
+}

src/path.h

@@ -29,6 +29,6 @@ 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_fitseq(path* p, trial* t, timestamp offset, timestamp testtime);
 int path_app_target(path* p, trial* t, int offset, int x, int y);
 #endif