robofsm: Prepare timing thread in advance and update position on display during wait...

diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc
index 363f44aaf336f5b8c825cdd688374a6dd9e7302a..ab563919d77c1bfec6a08360b26fbf6c54284533 100644 (file)
--- a/src/robofsm/competition.cc
+++ b/src/robofsm/competition.cc
@@ -89,6 +89,8 @@ void *timing_thread(void *arg)
 {
        struct timespec start;
 
+       sem_wait(&robot.start);
+
        clock_gettime(CLOCK_MONOTONIC, &start);
 #define WAIT(sec)                                                      \
        do {                                                            \
@@ -152,6 +154,13 @@ FSM_STATE(init)
        }
 }
 
+void set_initial_position()
+{
+       robot_set_est_pos_trans(0.16,
+                               PLAYGROUND_HEIGHT_M - 0.16,
+                               DEG2RAD(-45));
+}
+
 FSM_STATE(wait_for_start)
 {
        pthread_t thid;
@@ -159,24 +168,27 @@ FSM_STATE(wait_for_start)
        printf("COMPETITION mode set\n");
 #endif
        switch (FSM_EVENT) {
-#ifdef WAIT_FOR_START
                case EV_ENTRY:
+                       pthread_create(&thid, NULL, timing_thread, NULL);
+#ifdef WAIT_FOR_START
+                       FSM_TIMER(1000);
                        break;
-               case EV_START:
-#else
-               case EV_ENTRY:
-               case EV_START:
 #endif
+               case EV_START:
                        act_camera_off();
                        /* start competition timer */
-                       pthread_create(&thid, NULL, timing_thread, NULL);
+                       sem_post(&robot.start);
                        /* FIXME: */
-                       robot_set_est_pos_trans(0.16,
-                                               PLAYGROUND_HEIGHT_M - 0.16,
-                                               DEG2RAD(-45));
+                       set_initial_position();
                        FSM_TRANSITION(decide_what_next);
-                       break;
                case EV_TIMER:
+                       FSM_TIMER(1000);
+                       // We set initial position periodically in
+                       // order for it to be updated on the display
+                       // if the team color is changed during waiting
+                       // for start.
+                       set_initial_position();
+                       break;
                case EV_RETURN:
                case EV_GOAL_NOT_REACHABLE: // currently not used
                case EV_ACTION_DONE:

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index f21111da7a78b67117ad8d745c8f422c1cc237b0..f98323a1e349f14c9d34c6a62bbabfd0075d0cd2 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -230,6 +230,8 @@ int robot_start()
                goto err;
        }
 
+       sem_init(&robot.start, 0, 0);
+
        fsm_main_loop(&robot.main_loop);
 
 err:

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 59d444504fd4d4321bc17b0f988eebd3b514a6b4..6e16e8d481a13f85381bd636ee46c978eb55c27d 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -152,6 +152,7 @@ struct robot {
        void *new_trajectory;
        
        unsigned char isTrajectory;
+       sem_t start;
 
        struct fsm_main_loop main_loop;