Display competition time for some states

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index d9c1db3527d7b1919317f26d06286f35c4d8940a..d0ffaeca12d055b0d848ff9b6dc8d5683ed98459 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -25,6 +25,12 @@
  * "wait for start" states in particular strategies.
  ************************************************************************/
 
+#undef DBG_FSM_STATE
+#define DBG_FSM_STATE(name)    do { if (fsm->debug_states) printf("fsm %s %.1f: %s(%s)\n", \
+                                                                  fsm->debug_name, robot_current_time(), \
+                                                                  name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+
+
 static void set_initial_position()
 {
        robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,

diff --git a/src/robofsm/match-timing.c b/src/robofsm/match-timing.c
index fef733490f886ec1fce5f214a647dad80195342c..fc800c38475c49bb9e3c8d862d8dbc3fb7b00504 100644 (file)
--- a/src/robofsm/match-timing.c
+++ b/src/robofsm/match-timing.c
@@ -20,10 +20,10 @@
  * Competition timer. Stop robot when the timer exceeds.
  ********************************************************************** */
 
+static struct timespec start;
+       
 void *timing_thread(void *arg)
 {
-       struct timespec start;
-       
        sem_wait(&robot.start);
        clock_gettime(CLOCK_MONOTONIC, &start);
 #define WAIT(sec)                                                      \
@@ -49,3 +49,41 @@ void *timing_thread(void *arg)
        return NULL;
 }
 
+/* Subtract the `struct timespec' values X and Y,
+   storing the result in RESULT (result = x - y).
+   Return 1 if the difference is negative, otherwise 0.  */
+
+int
+timespec_subtract (struct timespec *result,
+                  struct timespec *x,
+                  struct timespec *y)
+{
+  /* Perform the carry for the later subtraction by updating Y. */
+  if (x->tv_nsec < y->tv_nsec) {
+    int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
+    y->tv_nsec -= 1000000000 * num_sec;
+    y->tv_sec += num_sec;
+  }
+  if (x->tv_nsec - y->tv_nsec > 1000000000) {
+    int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
+    y->tv_nsec += 1000000000 * num_sec;
+    y->tv_sec -= num_sec;
+  }
+
+  /* Compute the time remaining to wait.
+     `tv_nsec' is certainly positive. */
+  result->tv_sec = x->tv_sec - y->tv_sec;
+  result->tv_nsec = x->tv_nsec - y->tv_nsec;
+
+  /* Return 1 if result is negative. */
+  return x->tv_sec < y->tv_sec;
+}
+
+
+float robot_current_time()
+{
+       struct timespec now, diff;
+       clock_gettime(CLOCK_MONOTONIC, &now);
+       timespec_subtract(&diff, &now, &start);
+       return diff.tv_sec + diff.tv_nsec/1000000000.0;
+}

diff --git a/src/robofsm/match-timing.h b/src/robofsm/match-timing.h
index ea37919b7da7aadf2967254aa0e764b280c2c9f4..69a13a5753b14ad675c95f7c999c333c6460c251 100644 (file)
--- a/src/robofsm/match-timing.h
+++ b/src/robofsm/match-timing.h
@@ -6,6 +6,7 @@ extern "C" {
 #endif 
 
 void *timing_thread(void *arg);
+float robot_current_time();
 
 #ifdef __cplusplus
 }