Simulate

src/control.h

@@ -31,6 +31,9 @@ typedef struct vehicle vehicle;
 struct telemetry;
 typedef struct telemetry telemetry;
 
+struct simulated;
+typedef struct simulated simulated;
+
 struct map;
 typedef struct map map;
 
@@ -69,19 +72,30 @@ struct map {
 	GArray *solid_objects;
 };
 
+struct simulated {
+	timestamp ts;
+	vehicle vehicle;
+};
+
 struct trial {
 	map map;
 	timestamp last_ts;
 	GQueue telemetry;
-	vehicle vehicle; /* our view */
+	vehicle vehicle;
 	int alive, finished;
-	struct timeval started;
+	
+	simulated sim;
 	
 	/* internal */
+	struct timeval started;
 	int socket;
 	control_parser_ctx *parse_ctx;
 };
 
+/* extern */
+void simulate(trial *t);
+void godown(trial *t);
+
 /* trial */
 trial *trial_new(const char *hostname, const char *port);
 void trial_reset_run(trial *t);
@@ -116,7 +130,10 @@ telemetry* telemetry_new();
 
 /* Inline functions */
 
-#define SENDCMD(t, cmd) write(t->socket, cmd, sizeof(cmd)-1)
+#define SENDCMD(t, cmd) do { \
+	write(t->socket, cmd, sizeof(cmd)-1); \
+/* 	write(1, cmd, sizeof(cmd)-1); */ \
+} while(0)
 
 INLINE void vehicle_accel(trial *t) {
 	switch (t->vehicle.accel) {

src/control_parser.c

@@ -27,7 +27,7 @@ static timestamp extract_ts(context *ctx, char *fpc) {
 	return atoi(ctx->tmp->str);
 }
 
-#line 130 "control_parser.rl"
+#line 134 "control_parser.rl"
 
 
 
@@ -243,7 +243,7 @@ static const int control_parser_error = 0;
 
 static const int control_parser_en_main = 1;
 
-#line 133 "control_parser.rl"
+#line 137 "control_parser.rl"
 
 static int control_parser_has_error(context *ctx) {
 	return ctx->cs == control_parser_error;
@@ -260,7 +260,7 @@ void control_parser_new(trial *t) {
 	{
 	( ctx->cs) = control_parser_start;
 	}
-#line 145 "control_parser.rl"
+#line 149 "control_parser.rl"
 	ctx->buffer = g_string_sized_new(0);
 	ctx->tmp = g_string_sized_new(0);
 	ctx->mark = -1;
@@ -274,7 +274,7 @@ void control_parser_reset(trial *t) {
 	{
 	( ctx->cs) = control_parser_start;
 	}
-#line 154 "control_parser.rl"
+#line 158 "control_parser.rl"
 	g_string_truncate(ctx->tmp, 0);
 	ctx->mark = -1;
 	/* fprintf(stderr, "Parser reset\n"); */
@@ -399,13 +399,17 @@ _match:
 		} else {
 			/* fprintf(stderr, "time difference [ms]: %i\n", getcurts(t) - ctx->tm->ts); */
 		}
+		/* Reset simulation */
+		t->sim.ts = ctx->tm->ts;
+		t->sim.vehicle = t->vehicle;
+		
 		t->last_ts = ctx->tm->ts;
 		ctx->tm = NULL;
 		t->alive = 1;
 	}
 	break;
 	case 4:
-#line 55 "control_parser.rl"
+#line 59 "control_parser.rl"
 	{
 		ctx->tm->ts = ctx->ts;
 		ctx->tm->vehicle.x = ctx->x;
@@ -420,115 +424,115 @@ _match:
 	}
 	break;
 	case 5:
-#line 72 "control_parser.rl"
+#line 76 "control_parser.rl"
 	{ t->map.dx = extract_double(ctx, p); }
 	break;
 	case 6:
-#line 73 "control_parser.rl"
+#line 77 "control_parser.rl"
 	{ t->map.dy = extract_double(ctx, p); }
 	break;
 	case 7:
-#line 74 "control_parser.rl"
+#line 78 "control_parser.rl"
 	{ t->map.min_sensor = extract_double(ctx, p); }
 	break;
 	case 8:
-#line 75 "control_parser.rl"
+#line 79 "control_parser.rl"
 	{ t->map.max_sensor = extract_double(ctx, p); }
 	break;
 	case 9:
-#line 76 "control_parser.rl"
+#line 80 "control_parser.rl"
 	{ t->map.max_speed = extract_double(ctx, p); }
 	break;
 	case 10:
-#line 77 "control_parser.rl"
+#line 81 "control_parser.rl"
 	{ t->map.max_turn = extract_double(ctx, p); }
 	break;
 	case 11:
-#line 78 "control_parser.rl"
+#line 82 "control_parser.rl"
 	{ t->map.max_hard_turn = extract_double(ctx, p); }
 	break;
 	case 12:
-#line 82 "control_parser.rl"
+#line 86 "control_parser.rl"
 	{ ctx->x = extract_double(ctx, p); }
 	break;
 	case 13:
-#line 83 "control_parser.rl"
+#line 87 "control_parser.rl"
 	{ ctx->y = extract_double(ctx, p); }
 	break;
 	case 14:
-#line 84 "control_parser.rl"
+#line 88 "control_parser.rl"
 	{ ctx->r = extract_double(ctx, p); }
 	break;
 	case 15:
-#line 85 "control_parser.rl"
+#line 89 "control_parser.rl"
 	{ ctx->dir = extract_double(ctx, p); }
 	break;
 	case 16:
-#line 86 "control_parser.rl"
+#line 90 "control_parser.rl"
 	{ ctx->speed = extract_double(ctx, p); }
 	break;
 	case 17:
-#line 88 "control_parser.rl"
+#line 92 "control_parser.rl"
 	{ ctx->tm->vehicle.accel = ACCEL; }
 	break;
 	case 18:
-#line 89 "control_parser.rl"
+#line 93 "control_parser.rl"
 	{ ctx->tm->vehicle.accel = ROLL; }
 	break;
 	case 19:
-#line 90 "control_parser.rl"
+#line 94 "control_parser.rl"
 	{ ctx->tm->vehicle.accel = BRAKE; }
 	break;
 	case 20:
-#line 92 "control_parser.rl"
+#line 96 "control_parser.rl"
 	{ ctx->tm->vehicle.turn = TURN_HARD_LEFT; }
 	break;
 	case 21:
-#line 93 "control_parser.rl"
+#line 97 "control_parser.rl"
 	{ ctx->tm->vehicle.turn = TURN_LEFT; }
 	break;
 	case 22:
-#line 94 "control_parser.rl"
+#line 98 "control_parser.rl"
 	{ ctx->tm->vehicle.turn = TURN_STRAIGHT; }
 	break;
 	case 23:
-#line 95 "control_parser.rl"
+#line 99 "control_parser.rl"
 	{ ctx->tm->vehicle.turn = TURN_RIGHT; }
 	break;
 	case 24:
-#line 96 "control_parser.rl"
+#line 100 "control_parser.rl"
 	{ ctx->tm->vehicle.turn = TURN_HARD_RIGHT; }
 	break;
 	case 25:
-#line 98 "control_parser.rl"
+#line 102 "control_parser.rl"
 	{ ctx->ts = extract_ts(ctx, p); }
 	break;
 	case 26:
-#line 99 "control_parser.rl"
+#line 103 "control_parser.rl"
 	{ printf("Score %u\n", extract_ts(ctx, p)); }
 	break;
 	case 27:
-#line 101 "control_parser.rl"
+#line 105 "control_parser.rl"
 	{
 		object o = { BOLDER, ctx->x, ctx->y, ctx->r, 0, 0 };
 		g_array_append_val(ctx->tm->objects, o);
 	}
 	break;
 	case 28:
-#line 105 "control_parser.rl"
+#line 109 "control_parser.rl"
 	{
 		object o = { CRATER, ctx->x, ctx->y, ctx->r, 0, 0 };
 		g_array_append_val(ctx->tm->objects, o);
 	}
 	break;
 	case 29:
-#line 110 "control_parser.rl"
+#line 114 "control_parser.rl"
 	{
-		object o = { MARTIAN, ctx->x, ctx->y, ctx->r, ctx->dir, ctx->speed };
+		object o = { MARTIAN, ctx->x, ctx->y, MARTIAN_RADIUS, ctx->dir, ctx->speed };
 		g_array_append_val(ctx->tm->objects, o);
 	}
 	break;
-#line 532 "control_parser.c"
+#line 536 "control_parser.c"
 		}
 	}
 
@@ -540,7 +544,7 @@ _again:
 	_test_eof: {}
 	_out: {}
 	}
-#line 177 "control_parser.rl"
+#line 181 "control_parser.rl"
 
 		ctx->pos = p - ctx->buffer->str;
 		if (ctx->mark == -1) {

src/control_parser.rl

@@ -48,6 +48,10 @@ static timestamp extract_ts(context *ctx, char *fpc) {
 		} else {
 			/* fprintf(stderr, "time difference [ms]: %i\n", getcurts(t) - ctx->tm->ts); */
 		}
+		/* Reset simulation */
+		t->sim.ts = ctx->tm->ts;
+		t->sim.vehicle = t->vehicle;
+		
 		t->last_ts = ctx->tm->ts;
 		ctx->tm = NULL;
 		t->alive = 1;

src/main.c

@@ -4,8 +4,6 @@
 
 #include <stdio.h>
 
-void godown(trial *t);
-
 void trial_loop(trial *t) {
 // 	path *p;
 // 	int offset;
@@ -18,8 +16,9 @@ void trial_loop(trial *t) {
 // 		offset = path_app_target(p,t,offset,0,0);
 		while (t->alive) {
 // 			path_execute(t,p);
+			simulate(t);
 			godown(t);
-			if (-1 == trial_wait_for_input(t)) return;
+			if (-1 == trial_check_input(t)) return;
 		}
 		trial_matlab(t);
 		trial_reset_run(t);

src/potential.c

@@ -20,14 +20,14 @@ void godown(trial *t) {
 	double xpot, ypot, r, angle;
 	
 	/* todo: simulate movement */
-	v = t->vehicle;
+	v = t->sim.vehicle;
 	
 	tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
 	
 	r = sqrt(v.x*v.x + v.y*v.y);
 	if (r < 4) return; /* done */
-	xpot = v.x/r;
+	xpot = 0.2 * v.x/r;
-	ypot = v.y/r;
+	ypot = 0.2 * v.y/r;
 	
 	for (guint i = 0; i < tm->objects->len; i++) {
 		object *o = &g_array_index(tm->objects, object, i);
@@ -36,20 +36,21 @@ void godown(trial *t) {
 		
 		switch (o->type) {
 			case BOLDER:
-				sigma = 2 * o->rad;
+				sigma = 2;
 				h = 1;
 				break;
 			case CRATER:
-				sigma = 2 * o->rad;
+				sigma = 4;
-				h = 3;
+				h = 1;
 				break;
 			case MARTIAN:
-				sigma = 4 * o->rad;
+				sigma = 8;
 				h = 1;
 				break;
 		}
 		
-		r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) );
+		r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) ) - o->rad;
+// 		r = r*r*r * 10;
 		r = C1 * h / (sigma*sigma) / exp (r / (2*sigma) );
 		xpot += (o->x - v.x) * r;
 		ypot += (o->y - v.y) * r;
@@ -70,10 +71,11 @@ void godown(trial *t) {
 			vehicle_left(t);
 		}
 	}
+	fprintf(stderr, "Angle: %f, objects: %u\n", angle, tm->objects->len);
 	angle = abs(angle);
 	if (angle > 60) {
 		vehicle_break(t);
-	} else if (angle > 30) {
+	} else if (angle > 50) {
 		vehicle_roll(t);
 	} else {
 		vehicle_accel(t);

src/simulate.c

@@ -0,0 +1,35 @@
+
+#include "control.h"
+
+#include <math.h>
+
+#define LATENCY 20
+
+static double fixdir(double a) {
+	if (a < 0) a += 360;
+	else if (a > 360) a -= 360;
+	return a;
+}
+
+void simulate(trial *t) {
+	double move, angle;
+	
+	timestamp now = getcurts(t) + LATENCY, diff = now - t->sim.ts;
+	if (diff < 20) return;
+	
+/*	move = t->sim.vehicle.speed / 1000 * diff;
+	angle = t->sim.vehicle.dir*M_PI/180;
+	t->sim.vehicle.x += cos(angle)*move;
+	t->sim.vehicle.y += sin(angle)*move;*/
+	
+/*	switch (t->sim.vehicle.turn) {
+		case TURN_HARD_LEFT : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir + t->map.max_hard_turn); break;
+		case TURN_LEFT      : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir + t->map.max_turn); break;
+		case TURN_STRAIGHT  : break;
+		case TURN_RIGHT     : t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir - t->map.max_turn); break;
+		case TURN_HARD_RIGHT: t->sim.vehicle.dir = fixdir(t->sim.vehicle.dir - t->map.max_hard_turn); break;
+	}
+	
+	t->sim.vehicle.accel = t->vehicle.accel;
+	t->sim.vehicle.turn = t->vehicle.turn;*/
+}

src/wscript

@@ -9,6 +9,7 @@ main_source = '''
 	control_parser.c
 	path.c
 	potential.c
+	simulate.c
 '''
 
 def build(bld):