diff --git a/src/control.c b/src/control.c index 7652792..f54db25 100644 --- a/src/control.c +++ b/src/control.c @@ -1,2 +1,42 @@ #include "control.h" +static void telemetry_free(gpointer te, gpointer x) { + UNUSED(x); + g_array_free(te->objects, TRUE); + g_slice_free(telemetry, (telemetry*) te); +} + +static telemetry* telemetry_new() { + telemetry *te = g_slice_new0(telemetry); + te->objects = g_array_new(FALSE, FALSE, sizeof(object)); + return te; +} + +trial *trial_new() { + trial *t = g_slice_new0(trial); + g_queue_init(t->telemetry); + t->map.solid_objects = g_array_new(FALSE, FALSE, sizeof(object)); + return t; +} + +void trial_reset_run(trial *t) { + g_queue_foreach(t->telemetry, telemetry_free, NULL); + g_queue_clear(t->telementry); + t->last_ts = 0; +} + +void trial_wait_for_start(trial *t) { +} + +void trial_check_input(trial *t) { +} + +void trial_wait_for_input(trial *t) { +} + +void trial_free(trial *t) { + g_queue_foreach(t->telemetry, telemetry_free, NULL); + g_queue_clear(t->telementry); + g_array_free(t->map.solid_objects, TRUE); + g_slice_free(t); +} diff --git a/src/control.h b/src/control.h index 543b38c..5971d87 100644 --- a/src/control.h +++ b/src/control.h @@ -3,8 +3,11 @@ #include -typedef enum { ACCELERATING, ROLLING, BRAKING } accel_t; -typedef enum { TURN_HARD_LEFT, TURN_LEFT, TURN_STRAIGT, TURN_RIGHT, TURN_HARD_RIGHT } turn_t; +#define INLINE static inline +#define UNUSED(x) ((void)(x)) + +typedef enum { ACCEL, ROLL, BREAK } accel_t; +typedef enum { TURN_HARD_LEFT, TURN_LEFT, TURN_STRAIGHT, TURN_RIGHT, TURN_HARD_RIGHT } turn_t; typedef enum { BOLDER, CRATER, MARTIAN } object_t; @@ -43,7 +46,7 @@ struct vehicle { struct telemetry { timestamp ts; vehicle vehicle; - GArray objects; + GArray *objects; }; struct map { @@ -52,22 +55,116 @@ struct map { float min_sensor, max_sensor; float max_speed, max_turn, max_turn_hard; - GList solid_objects; + GArray *solid_objects; }; struct trial { map map; timestamp last_ts; - GList telemetry_data; + GQueue telemetry_data; vehicle vehicle; /* our view */ + + /* internal */ + int socket; }; /* trial */ trial *trial_new(); -void trial_reset_run(trial *trial); -void trial_wait_for_start(trial *trial); -void trial_check_input(trial *trial); -void trial_wait_for_input(trial *trial); -void trial_free(trial *trial); +void trial_reset_run(trial *t); +void trial_wait_for_start(trial *t); +void trial_check_input(trial *t); +void trial_wait_for_input(trial *t); +void trial_free(trial *t); + +INLINE void vehicle_accel(trial *t); +INLINE void vehicle_roll(trial *t); +INLINE void vehicle_break(trial *t); + +INLINE void vehicle_hard_left(trial *t); +INLINE void vehicle_left(trial *t); +INLINE void vehicle_straight(trial *t); +INLINE void vehicle_right(trial *t); +INLINE void vehicle_hard_right(trial *t); + +/* Inline functions */ + +#define SENDCMD(t, cmd) write(t->socket, cmd, sizeof(cmd)-1) + +INLINE void vehicle_accel(trial *t) { + switch (t->vehicle.accel) { + case ACCEL: return; + case ROLL: SENDCMD(t, "a;"); break; + case BREAK: SENDCMD(t, "a;a;"); break; + } + t->vehicle.accel = ACCEL; +} +INLINE void vehicle_roll(trial *t) { + switch (t->vehicle.accel) { + case ACCEL: SENDCMD(t, "b;"); break; + case ROLL: return; + case BREAK: SENDCMD(t, "a;"); break; + } + t->vehicle.accel = ROLL; +} +INLINE void vehicle_break(trial *t) { + switch (t->vehicle.accel) { + case ACCEL: SENDCMD(t, "b;b;"); break; + case ROLL: SENDCMD(t, "b;"); break; + case BREAK: return; + } + t->vehicle.accel = BREAK; +} + +INLINE void vehicle_hard_left(trial *t) { + switch (t->vehicle.turn) { + case TURN_HARD_LEFT: return; + case TURN_LEFT: SENDCMD(t, "l;"); break; + case TURN_STRAIGHT: SENDCMD(t, "l;l;"); break; + case TURN_RIGHT: SENDCMD(t, "l;l;l;"); break; + case TURN_HARD_RIGHT: SENDCMD(t, "l;l;l;l;"); break; + } + t->vehicle.turn = TURN_HARD_LEFT; +} + +INLINE void vehicle_left(trial *t) { + switch (t->vehicle.turn) { + case TURN_HARD_LEFT: SENDCMD(t, "r;"); break; + case TURN_LEFT: return; + case TURN_STRAIGHT: SENDCMD(t, "l;"); break; + case TURN_RIGHT: SENDCMD(t, "l;l;"); break; + case TURN_HARD_RIGHT: SENDCMD(t, "l;l;l;"); break; + } + t->vehicle.turn = TURN_LEFT; +} +INLINE void vehicle_straight(trial *t) { + switch (t->vehicle.turn) { + case TURN_HARD_LEFT: SENDCMD(t, "r;r;"); break; + case TURN_LEFT: SENDCMD(t, "r;"); break; + case TURN_STRAIGHT: return; + case TURN_RIGHT: SENDCMD(t, "l;"); break; + case TURN_HARD_RIGHT: SENDCMD(t, "l;l;"); break; + } + t->vehicle.turn = TURN_STRAIGHT; +} +INLINE void vehicle_right(trial *t) { + switch (t->vehicle.turn) { + case TURN_HARD_LEFT: SENDCMD(t, "r;r;r;"); break; + case TURN_LEFT: SENDCMD(t, "r;r;"); break; + case TURN_STRAIGHT: SENDCMD(t, "r;"); break; + case TURN_RIGHT: return; + case TURN_HARD_RIGHT: SENDCMD(t, "l;"); break; + } + t->vehicle.turn = TURN_RIGHT; +} +INLINE void vehicle_hard_right(trial *t) { + switch (t->vehicle.turn) { + case TURN_HARD_LEFT: SENDCMD(t, "r;r;r;r;"); break; + case TURN_LEFT: SENDCMD(t, "r;r;r;"); break; + case TURN_STRAIGHT: SENDCMD(t, "r;r;"); break; + case TURN_RIGHT: SENDCMD(t, "r;"); break; + case TURN_HARD_RIGHT: return; + } + t->vehicle.turn = TURN_HARD_RIGHT; +} #endif