src/robofsm/match-timing.c

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