Merge branch 'strobmir'

diff --combined src/robofsm/fsmmove.cc
index d8d227a087b87889756327a50eac287a0dee2344,f46cc39dacb400d15f9ddde1448a8ba924df5ba7..7f2e99de645be29cb7a744a016abba8010bef72b
--- 1/src/robofsm/fsmmove.cc
--- 2/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@@ -11,7 -11,7 +11,7 @@@
  #include <time.h>
  #include "trgen.h"
  #include "balet.h"
 -#include <robodata.h>
 +#include "robodata.h"
  #include <robot.h>
  #include <pthread.h>
  #include <path_planner.h>
@@@ -24,9 -24,12 +24,12 @@@
  #include <robomath.h>
  #include "motion-control.h"
  #include <hokuyo.h>
+ #include <ul_log.h>
+ 
+ UL_LOG_CUST(ulogd_fsmmove); /* Log domain name = ulogd + name of the file */
  
  #ifdef MOTION_DEBUG
-    #define DBG(format, ...) printf(format, ##__VA_ARGS__)
+    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  #else
     #define DBG(format, ...)
  #endif
@@@ -184,7 -187,7 +187,7 @@@ static enum target_status new_trajector
        Pos pos;
  
        if (!t) {
-               printf("ERROR: No trajectory\n");
+               ul_logerr("ERROR: No trajectory\n");
                return TARGET_ERROR;
        }
        // FIXME: This is duplicite code with new_goal. Remove this
@@@ -248,7 -251,7 +251,7 @@@ recalculate_trajectory(void
        case TARGET_OK:
                break;                                  
        case TARGET_ERROR:                              
-               printf("Target error on recalculation_in_progress\n");          
+               ul_logerr("Target error on recalculation_in_progress\n");               
                break;                                  
        case TARGET_INACC:                      
                break;
@@@ -291,7 -294,7 +294,7 @@@ FSM_STATE(wait_for_command
                        switch (ret) {
                                case TARGET_OK:    FSM_TRANSITION(movement); break;
                                case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
-                               case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
+                               case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
                        }
                        free(FSM_EVENT_PTR);
                        break;
@@@ -340,7 -343,7 +343,7 @@@ FSM_STATE(movement
                                case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break; 
                                // Fatal error during planning
                                case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
-                                                  printf("Target error\n");
+                                                  ul_logerr("Target error\n");
                                                   /* FIXME (Filip): shouldn't transition to wait_for_command be here?
                                                      No, probably not, if an error occurs, robot won't move any more
                                                      and we can recognize the error. */
@@@ -421,7 -424,7 +424,7 @@@ FSM_STATE(lost
                        switch (recalculate_trajectory()) {
                                case TARGET_OK:    FSM_TRANSITION(movement); break;
                                case TARGET_INACC: FSM_TRANSITION(wait_and_try_again); break;
-                               case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
+                               case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
                        }
                        break;
                case EV_MOVE_STOP:
@@@ -455,12 -458,12 +458,12 @@@ FSM_STATE(wait_and_try_again
                                // Target inaccessible because of an obstacle
                                case TARGET_INACC:
                                        FSM_TRANSITION(wait_and_try_again);
-                                       printf("Inaccessible\n");
+                                       ul_logerr("Inaccessible\n");
                                        FSM_TIMER(1000);
                                        /* FIXME (Filip): this could happen forever */
                                        break; 
                                // Fatal error during planning
-                               case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
+                               case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); ul_logerr("Target error\n"); break;
                        }
                        break;
                case EV_TRAJECTORY_DONE:

diff --combined src/robofsm/homologation.cc
index 367b816b03c41ea2d0dddbd47b9f89580dc43501,b7d21ca88b53cef4da2b9aedad777a88b1f13848..4b376ad9bea26c0594aa91b65dc3527a12265ad3
--- 1/src/robofsm/homologation.cc
--- 2/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@@ -13,6 -13,7 +13,6 @@@
  #endif
  
  #define FSM_MAIN
 -#include <robodata.h>
  #include <robot.h>
  #include <fsm.h>
  #include <unistd.h>
@@@ -29,6 -30,9 +29,9 @@@
  #include <trgen.h>
  #include "match-timing.h"
  #include "eb2010misc.h"
+ #include <ul_log.h>
+ 
+ UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
  
  /************************************************************************
   * Trajectory constraints used, are initialized in the init state
@@@ -103,14 -107,14 +106,14 @@@ FSM_STATE(wait_for_start
  {
        pthread_t thid;
        #ifdef WAIT_FOR_START
-               printf("WAIT_FOR_START mode set\n");
+               ul_logdeb("WAIT_FOR_START mode set\n");
        #else
-               printf("WAIT_FOR_START mode NOT set\n");
+               ul_logdeb("WAIT_FOR_START mode NOT set\n");
        #endif
        #ifdef COMPETITION
-               printf("COMPETITION mode set\n");
+               ul_logdeb("COMPETITION mode set\n");
        #else
-               printf("COMPETITION mode NOT set\n");
+               ul_logdeb("COMPETITION mode NOT set\n");
        #endif
        switch (FSM_EVENT) {
                case EV_ENTRY:
@@@ -165,7 -169,7 +168,7 @@@ FSM_STATE(zvedej_vidle
                case EV_TIMER:
                        FSM_TIMER(500);
                        act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED);
-                       printf("--------------------cnt: %d\n", cnt);
+                       ul_logdeb("--------------------cnt: %d\n", cnt);
                        cnt++;
                        if(cnt >= 3) {
                                robot_exit();
@@@ -365,7 -369,7 +368,7 @@@ FSM_STATE(approach_next_corn
                case EV_ENTRY: {
                                double x, y, phi;
                                robot_get_est_pos(&x, &y, &phi);
-                               printf("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
+                               ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
                                        cnt, x, y, phi);
  
                                corn_to_get = choose_next_corn();
@@@ -408,7 -412,7 +411,7 @@@ FSM_STATE(rush_the_corn
                        } else {
                                x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
                        }
-                       printf("x = %.3f, %.3f \n", x, corn_to_get->position.y);
+                       ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y);
                        remove_wall_around_corn(x, corn_to_get->position.y);
                        robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
                        where_to_go = TURN_AROUND;

diff --combined src/robofsm/motion-control.cc
index f5ab11ac41755997de93aeeddcf6871ccf5edf13,48a05510908583bfaf895cbecc5f87222c1f7c89..551dae0ec81a44a5d55a46e84363b595014ca274
--- 1/src/robofsm/motion-control.cc
--- 2/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@@ -11,7 -11,7 +11,7 @@@
  //#define MOTION_DEBUG
  
  #ifdef MOTION_DEBUG
-     #define DBG(format, ...) printf(format, ##__VA_ARGS__)
+     #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
      #define DBGflush() fflush(stdout)
  #else
      #define DBG(format, ...)
@@@ -22,7 -22,7 +22,7 @@@
  #include <time.h>
  #include "trgen.h"
  #include "balet.h"
 -#include <robodata.h>
 +#include "robodata.h"
  #include <robot.h>
  #include <pthread.h>
  #include <path_planner.h>
@@@ -33,6 -33,9 +33,9 @@@
  #include <map.h>
  #include "robot_config.h"
  #include <robomath.h>
+ #include <ul_log.h>
+ 
+ UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file */
  
  #define MOTION_CONTROL
  #include "motion-control.h"
@@@ -230,7 -233,7 +233,7 @@@ static void do_control(
  #ifdef MOTION_PRINT_REF
                static double last_t;
                if (t < last_t) last_t = t; // Switched to a new trajectory
-               printf("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f, time=%lf dt=%lf \n", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega, t, t-last_t);
+               ul_logdeb("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f  omega=%-4.02f, time=%lf dt=%lf \n", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega, t, t-last_t);
                fflush(stdout);
                last_t = t;
  #endif

diff --combined src/robofsm/movehelper.cc
index 20eb2189ad49e71e541f7dbeb78ebd7445a575d4,1a31b4f3d44894f1c0e39ffec1f6860a944fe213..613b27f9c1b6049aac516749867280b4fdd5e4f9
--- 1/src/robofsm/movehelper.cc
--- 2/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@@ -9,7 -9,7 +9,7 @@@
   */
  
  #include <trgen.h>
 -#include <robodata.h>
 +#include "robodata.h"
  #include <robot.h>
  #include <movehelper.h>
  #include <path_simplifier.h>
@@@ -21,6 -21,9 +21,9 @@@
  #include <robomath.h>
  #include <mcl_robot.h>
  #include <string.h>
+ #include <ul_log.h>
+ 
+ UL_LOG_CUST(ulogd_movehelper); /* Log domain name = ulogd + name of the file */
  
  #if 0
  struct TrajectoryConstraints trajectoryConstraintsDefault = {
@@@ -174,7 -177,7 +177,7 @@@ void robot_trajectory_add_final_point_n
                FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
                robot.new_trajectory = NULL;
        } else {
-               printf("%s: Error - No trajectory\n", __FUNCTION__);
+               ul_logerr("%s: Error - No trajectory\n", __FUNCTION__);
        }
  }
  

diff --combined src/robofsm/robot_orte.c
index 0a586d28bd1099ec519e0622fcb573797066baa6,0040f686097d0c6d87476cf21a10bfc02714ef7c..19f296e417227a6687e2cbab6fd5dc475437f036
--- 1/src/robofsm/robot_orte.c
--- 2/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@@ -17,7 -17,7 +17,7 @@@
  
  #include <orte.h>
  #include <roboorte_robottype.h>
 -#include <robodata.h>
 +#include "robodata.h"
  #include <robot.h>
  #include <movehelper.h>
  #include <math.h>
@@@ -26,9 -26,12 +26,12 @@@
  #include <string.h>
  #include <can_msg_def.h>
  #include <actuators.h>
+ #include <ul_log.h>
+ 
+ UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
  
  #ifdef ORTE_DEBUG
-    #define DBG(format, ...) printf(format, ##__VA_ARGS__)
+    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  #else
     #define DBG(format, ...)
  #endif
@@@ -258,7 -261,7 +261,7 @@@ void rcv_robot_cmd_cb(const ORTERecvInf
                                        if (!instance->start_condition) {
                                                robot.status[COMPONENT_START] = STATUS_WARNING;
                                                robot.start_state = START_PLUGGED_IN;
-                                               printf("START_PLUGGED_IN\n");
+                                               ul_logmsg("START_PLUGGED_IN\n");
                                        }
                                        break;
  
@@@ -268,7 -271,7 +271,7 @@@
                                        if (instance->start_condition) {
                                                FSM_SIGNAL(MAIN, EV_START, NULL);
                                                robot.start_state = COMPETITION_STARTED;
-                                               printf("STARTED\n");
+                                               ul_logmsg("STARTED\n");
                                        }
                                        break;