src/robofsm/match-timing.c

   1 #include "match-timing.h"
   2 #include "robot.h"
   3
   4 #ifdef COMPETITION
   5 #define COMPETITION_TIME_DEFAULT        90
   6 #define TIME_TO_DEPOSITE_DEFAULT        60
   7 #else
   8 #define COMPETITION_TIME_DEFAULT        90
   9 #define TIME_TO_DEPOSITE_DEFAULT        60
  10 #endif
  11
  12 /* competition time in seconds */
  13 #define COMPETITION_TIME        COMPETITION_TIME_DEFAULT
  14 #define TIME_TO_DEPOSITE        TIME_TO_DEPOSITE_DEFAULT
  15 /* competition time in seconds */
  16
  17
  18
  19 /** *********************************************************************
  20  * Competition timer. Stop robot when the timer exceeds.
  21  ********************************************************************** */
  22
  23 static struct timespec start;
  24
  25 void *timing_thread(void *arg)
  26 {
  27         sem_wait(&robot.start);
  28         clock_gettime(CLOCK_MONOTONIC, &start);
  29 #define WAIT(sec)                                                       \
  30         do {                                                            \
  31                 struct timespec t;                                      \
  32                 t.tv_sec = start.tv_sec+sec;                            \
  33                 t.tv_nsec = start.tv_nsec;                              \
  34                 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); \
  35         } while(0)
  36
  37 //      WAIT(5);
  38 //      // microswitch (backside opponent detector), ignore it while at starting point
  39 //      robot.use_back_switch = true;
  40 //      printf("Back switch not ignored\n");
  41
  42         WAIT(TIME_TO_DEPOSITE);
  43         robot.short_time_to_end = true;
  44
  45         WAIT(COMPETITION_TIME);
  46         printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
  47         robot_exit();
  48
  49         return NULL;
  50 }
  51
  52 /* Subtract the `struct timespec' values X and Y,
  53    storing the result in RESULT (result = x - y).
  54    Return 1 if the difference is negative, otherwise 0.  */
  55
  56 int
  57 timespec_subtract (struct timespec *result,
  58                    struct timespec *x,
  59                    struct timespec *y)
  60 {
  61   /* Perform the carry for the later subtraction by updating Y. */
  62   if (x->tv_nsec < y->tv_nsec) {
  63     int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
  64     y->tv_nsec -= 1000000000 * num_sec;
  65     y->tv_sec += num_sec;
  66   }
  67   if (x->tv_nsec - y->tv_nsec > 1000000000) {
  68     int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
  69     y->tv_nsec += 1000000000 * num_sec;
  70     y->tv_sec -= num_sec;
  71   }
  72
  73   /* Compute the time remaining to wait.
  74      `tv_nsec' is certainly positive. */
  75   result->tv_sec = x->tv_sec - y->tv_sec;
  76   result->tv_nsec = x->tv_nsec - y->tv_nsec;
  77
  78   /* Return 1 if result is negative. */
  79   return x->tv_sec < y->tv_sec;
  80 }
  81
  82
  83 float robot_current_time()
  84 {
  85         struct timespec now, diff;
  86         clock_gettime(CLOCK_MONOTONIC, &now);
  87         timespec_subtract(&diff, &now, &start);
  88         return diff.tv_sec + diff.tv_nsec/1000000000.0;
  89 }