You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

228 lines
5.2 KiB

  1. #ifndef _CONTROL_H
  2. #define _CONTROL_H
  3. #include "config.h"
  4. #include <unistd.h>
  5. #include <sys/time.h>
  6. #include <glib.h>
  7. #include <stdio.h>
  8. #define INLINE static inline
  9. #define UNUSED(x) ((void)(x))
  10. #define VEHICLE_RADIUS 0.5
  11. #define HOMEBASE_RADIUS 5
  12. #define MARTIAN_RADIUS 0.4
  13. typedef enum { RUN_ERROR, RUN_DONE, RUN_GO_ON, RUN_FINISHED } run_t;
  14. typedef enum { ACCEL, ROLL, BRAKE } accel_t;
  15. typedef enum { TURN_HARD_LEFT, TURN_LEFT, TURN_STRAIGHT, TURN_RIGHT, TURN_HARD_RIGHT } turn_t;
  16. typedef enum { BOLDER, CRATER, MARTIAN } object_t;
  17. struct object;
  18. typedef struct object object;
  19. struct vehicle;
  20. typedef struct vehicle vehicle;
  21. struct telemetry;
  22. typedef struct telemetry telemetry;
  23. struct simulated;
  24. typedef struct simulated simulated;
  25. struct map;
  26. typedef struct map map;
  27. struct trial;
  28. typedef struct trial trial;
  29. typedef unsigned int timestamp;
  30. #include "control_parser.h"
  31. struct object {
  32. object_t type;
  33. double x, y, rad;
  34. double dir, speed;
  35. };
  36. struct vehicle {
  37. accel_t accel;
  38. turn_t turn;
  39. double x, y;
  40. double dir, speed;
  41. };
  42. struct telemetry {
  43. timestamp ts;
  44. vehicle vehicle;
  45. GArray *objects;
  46. };
  47. struct map {
  48. float dx, dy;
  49. timestamp limit;
  50. float min_sensor, max_sensor;
  51. float max_speed, max_turn, max_hard_turn;
  52. GHashTable *solid_objects;
  53. };
  54. struct simulated {
  55. telemetry tm;
  56. guint steps;
  57. };
  58. struct trial {
  59. map map;
  60. timestamp last_ts;
  61. GQueue telemetry;
  62. vehicle vehicle;
  63. int alive, finished;
  64. simulated sim;
  65. int runcnt;
  66. /* internal */
  67. struct timeval started;
  68. int socket;
  69. control_parser_ctx *parse_ctx;
  70. };
  71. /* extern */
  72. void simulate(trial *t, timestamp ts);
  73. void godown(trial *t);
  74. void goradar(trial *t);
  75. /* trial */
  76. trial *trial_new(const char *hostname, const char *port);
  77. void trial_reset_run(trial *t);
  78. int trial_wait_for_start(trial *t);
  79. int trial_check_input(trial *t);
  80. int trial_wait_for_input(trial *t);
  81. void trial_free(trial *t);
  82. object* object_new(object_t type, double x, double y, double rad, double dir, double speed);
  83. /* debugging stuff */
  84. void object_print(object *o);
  85. void vehicle_print(vehicle *v);
  86. void telemetry_print(telemetry *t);
  87. void trial_print(trial *t);
  88. void trial_matlab(trial *t);
  89. void map_print(map *m);
  90. INLINE void vehicle_accel(trial *t);
  91. INLINE void vehicle_roll(trial *t);
  92. INLINE void vehicle_break(trial *t);
  93. INLINE void vehicle_hard_left(trial *t);
  94. INLINE void vehicle_left(trial *t);
  95. INLINE void vehicle_straight(trial *t);
  96. INLINE void vehicle_right(trial *t);
  97. INLINE void vehicle_hard_right(trial *t);
  98. INLINE timestamp getcurts(trial *t);
  99. /* needed by parser */
  100. telemetry* telemetry_new();
  101. /* Inline functions */
  102. #define SENDCMD(t, cmd) do { \
  103. write(t->socket, cmd, sizeof(cmd)-1); \
  104. /* write(1, cmd, sizeof(cmd)-1); */ \
  105. } while(0)
  106. INLINE void vehicle_accel(trial *t) {
  107. switch (t->vehicle.accel) {
  108. case ACCEL: return;
  109. case ROLL: SENDCMD(t, "a;"); break;
  110. case BRAKE: SENDCMD(t, "a;a;"); break;
  111. }
  112. t->vehicle.accel = ACCEL;
  113. }
  114. INLINE void vehicle_roll(trial *t) {
  115. switch (t->vehicle.accel) {
  116. case ACCEL: SENDCMD(t, "b;"); break;
  117. case ROLL: return;
  118. case BRAKE: SENDCMD(t, "a;"); break;
  119. }
  120. t->vehicle.accel = ROLL;
  121. }
  122. INLINE void vehicle_break(trial *t) {
  123. switch (t->vehicle.accel) {
  124. case ACCEL: SENDCMD(t, "b;b;"); break;
  125. case ROLL: SENDCMD(t, "b;"); break;
  126. case BRAKE: return;
  127. }
  128. t->vehicle.accel = BRAKE;
  129. }
  130. INLINE void vehicle_hard_left(trial *t) {
  131. switch (t->vehicle.turn) {
  132. case TURN_HARD_LEFT: return;
  133. case TURN_LEFT: SENDCMD(t, "l;"); break;
  134. case TURN_STRAIGHT: SENDCMD(t, "l;l;"); break;
  135. case TURN_RIGHT: SENDCMD(t, "l;l;l;"); break;
  136. case TURN_HARD_RIGHT: SENDCMD(t, "l;l;l;l;"); break;
  137. }
  138. t->vehicle.turn = TURN_HARD_LEFT;
  139. }
  140. INLINE void vehicle_left(trial *t) {
  141. switch (t->vehicle.turn) {
  142. case TURN_HARD_LEFT: SENDCMD(t, "r;"); break;
  143. case TURN_LEFT: return;
  144. case TURN_STRAIGHT: SENDCMD(t, "l;"); break;
  145. case TURN_RIGHT: SENDCMD(t, "l;l;"); break;
  146. case TURN_HARD_RIGHT: SENDCMD(t, "l;l;l;"); break;
  147. }
  148. t->vehicle.turn = TURN_LEFT;
  149. }
  150. INLINE void vehicle_straight(trial *t) {
  151. switch (t->vehicle.turn) {
  152. case TURN_HARD_LEFT: SENDCMD(t, "r;r;"); break;
  153. case TURN_LEFT: SENDCMD(t, "r;"); break;
  154. case TURN_STRAIGHT: return;
  155. case TURN_RIGHT: SENDCMD(t, "l;"); break;
  156. case TURN_HARD_RIGHT: SENDCMD(t, "l;l;"); break;
  157. }
  158. t->vehicle.turn = TURN_STRAIGHT;
  159. }
  160. INLINE void vehicle_right(trial *t) {
  161. switch (t->vehicle.turn) {
  162. case TURN_HARD_LEFT: SENDCMD(t, "r;r;r;"); break;
  163. case TURN_LEFT: SENDCMD(t, "r;r;"); break;
  164. case TURN_STRAIGHT: SENDCMD(t, "r;"); break;
  165. case TURN_RIGHT: return;
  166. case TURN_HARD_RIGHT: SENDCMD(t, "l;"); break;
  167. }
  168. t->vehicle.turn = TURN_RIGHT;
  169. }
  170. INLINE void vehicle_hard_right(trial *t) {
  171. switch (t->vehicle.turn) {
  172. case TURN_HARD_LEFT: SENDCMD(t, "r;r;r;r;"); break;
  173. case TURN_LEFT: SENDCMD(t, "r;r;r;"); break;
  174. case TURN_STRAIGHT: SENDCMD(t, "r;r;"); break;
  175. case TURN_RIGHT: SENDCMD(t, "r;"); break;
  176. case TURN_HARD_RIGHT: return;
  177. }
  178. t->vehicle.turn = TURN_HARD_RIGHT;
  179. }
  180. INLINE timestamp getcurts(trial *t) {
  181. struct timeval tv;
  182. gettimeofday(&tv, NULL);
  183. timestamp ts = (tv.tv_sec - t->started.tv_sec) * 1000 + (tv.tv_usec - t->started.tv_usec) / 1000;
  184. // printf("Timestamp: %u\n", ts);
  185. return ts;
  186. }
  187. #endif