robofsm: Update robot_orte to accept new interface and new FSM

diff --git a/src/robofsm/robot_orte.cc b/src/robofsm/robot_orte.cc
index bc952c4f0053fcd1f6a8ebf303b8f200e50d31ba..c5e528871e27ae480b99f069ebb0b6a2b87f923e 100644 (file)
--- a/src/robofsm/robot_orte.cc
+++ b/src/robofsm/robot_orte.cc
@@ -27,6 +27,8 @@
 #include <can_msg_def.h>
 #include <actuators.h>
 #include <ul_log.h>
+#include "guard.hpp"
+#include "events.h"
 
 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
 
@@ -72,9 +74,8 @@ void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
 {
        struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 
-       ROBOT_LOCK(ref_pos);
+       Guard g(robot.lock_ref_pos);
        *instance = robot.ref_pos;
-       ROBOT_UNLOCK(ref_pos);
 }
 
 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
@@ -82,9 +83,8 @@ void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
 {
        struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 
-       ROBOT_LOCK(est_pos_odo);
+       Guard g(robot.lock_est_pos_odo);
        *instance = robot.est_pos_odo;
-       ROBOT_UNLOCK(est_pos_odo);
 }
 
 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
@@ -92,9 +92,8 @@ void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
 {
        struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 
-       ROBOT_LOCK(est_pos_indep_odo);
+       Guard g(robot.lock_est_pos_indep_odo);
        *instance = robot.est_pos_indep_odo;
-       ROBOT_UNLOCK(est_pos_indep_odo);
 }
 
 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
@@ -102,7 +101,7 @@ static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
 {
        struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 
-        robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
+        robot.get_est_pos(instance->x, instance->y, instance->phi);
 }
 
 static void send_system_status_cb(const ORTESendInfo *info, void *vinstance,
@@ -141,9 +140,10 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA:
                        if (first_run) {
-                               ROBOT_LOCK(odo_data);
-                               robot.odo_data = *instance;
-                               ROBOT_UNLOCK(odo_data);
+                               {
+                                       Guard g(robot.lock);
+                                       robot.odo_data = *instance;
+                               }
                                first_run = false;
                                break;
                        }
@@ -153,17 +153,17 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
 
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
-
-                       ROBOT_LOCK(est_pos_indep_odo);
-                       double a = robot.est_pos_indep_odo.phi;
-                       robot.est_pos_indep_odo.x += dtang*cos(a);
-                       robot.est_pos_indep_odo.y += dtang*sin(a);
-                       robot.est_pos_indep_odo.phi += dphi;
-                       ROBOT_UNLOCK(est_pos_indep_odo);
-
-                       ROBOT_LOCK(odo_data);
-                       robot.odo_data = *instance;
-                       ROBOT_UNLOCK(odo_data);
+                       {
+                               Guard g(robot.lock_est_pos_indep_odo);
+                               double a = robot.est_pos_indep_odo.phi;
+                               robot.est_pos_indep_odo.x += dtang*cos(a);
+                               robot.est_pos_indep_odo.y += dtang*sin(a);
+                               robot.est_pos_indep_odo.phi += dphi;
+                       }
+                       {
+                               Guard g(robot.lock);
+                               robot.odo_data = *instance;
+                       }
 
                        robot.indep_odometry_works = true;
 
@@ -195,9 +195,10 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA:
                        if (first_run) {
-                               ROBOT_LOCK(motion_irc);
-                               robot.motion_irc = *instance;
-                               ROBOT_UNLOCK(motion_irc);
+                               {
+                                       Guard g(robot.lock);
+                                       robot.motion_irc = *instance;
+                               }
                                first_run = false;
                                break;
                        }
@@ -207,19 +208,21 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 
                        dtang = (dleft + dright) / 2.0;
                        dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
-
-                       ROBOT_LOCK(est_pos_odo);
-                       double a = robot.est_pos_odo.phi;
-                       robot.est_pos_odo.x += dtang*cos(a);
-                       robot.est_pos_odo.y += dtang*sin(a);
-                       robot.est_pos_odo.phi += dphi;
-                       ROBOT_UNLOCK(est_pos_odo);
+                       
+                       {
+                               Guard g(robot.lock_est_pos_odo);
+                               double a = robot.est_pos_odo.phi;
+                               robot.est_pos_odo.x += dtang*cos(a);
+                               robot.est_pos_odo.y += dtang*sin(a);
+                               robot.est_pos_odo.phi += dphi;
+                       }
 
                        /* locking should not be needed, but... */
-                       ROBOT_LOCK(motion_irc);
-                       robot.motion_irc = *instance;
-                       robot.motion_irc_received = 1;
-                       ROBOT_UNLOCK(motion_irc);
+                       {
+                               Guard g(robot.lock);
+                               robot.motion_irc = *instance;
+                               robot.motion_irc_received = 1;
+                       }
 
                        robot.odometry_works = true;
 
@@ -312,7 +315,7 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                                        robot.status[COMPONENT_START] = STATUS_OK;
                                        /* Competition starts when plugged out, check component status before start */
                                        if ((instance->start_condition == COMPETITION_STARTED) && !check_prestart_status()) {
-                                               FSM_SIGNAL(MAIN, EV_START, NULL);
+                                               robot.sched.queue_event(robot.MAIN, new evStart());
                                                robot.start_state = COMPETITION_STARTED;
                                                ul_logmsg("STARTED\n");
                                        }
@@ -324,7 +327,7 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                                        static int num = 0;
                                        if (instance->start_condition == START_PLUGGED_IN) {
                                                if (++num == 10)
-                                                       robot_exit();
+                                                       robot.robot_exit();
                                        }
                                        break;
                                }
@@ -345,10 +348,11 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 
        switch (info->status) {
                case NEW_DATA: {
-                       ROBOT_LOCK(hokuyo);
-                       robot.hokuyo = *instance;
-                       robot.status[COMPONENT_HOKUYO] = STATUS_OK;
-                       ROBOT_UNLOCK(hokuyo);
+                       {
+                               Guard g(robot.lock);
+                               robot.hokuyo = *instance;
+                               robot.status[COMPONENT_HOKUYO] = STATUS_OK;
+                       }
                        if(!robot.ignore_hokuyo)
                        {
                                update_map_hokuyo(instance);
@@ -372,15 +376,16 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA: {
                         if (instance->error) {
-                                robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
+                               robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
                         } else if (instance->data_valid && instance->data_valid != last_response) {
-                                ROBOT_LOCK(camera_result);
-                                robot.target_valid = instance->target_valid;
-                                ROBOT_UNLOCK(camera_result);
-                                FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
-                                printf("robot_orte: valid: %d\n", instance->target_valid);
+                               {
+                                       Guard g(robot.lock_camera);
+                                       robot.target_valid = instance->target_valid;
+                               }
+                               robot.sched.queue_event(robot.MAIN, new evCameraDone());
+                               printf("robot_orte: valid: %d\n", instance->target_valid);
                                 
-                                robot.status[COMPONENT_CAMERA] = STATUS_OK;
+                               robot.status[COMPONENT_CAMERA] = STATUS_OK;
                         }
                         last_response = instance->data_valid;
                        break;
@@ -411,7 +416,7 @@ void rcv_crane_status_cb(const ORTERecvInfo *info, void *vinstance,
 
                        if (instance->response != last_response &&
                            instance->response == act_crane_get_last_reqest())
-                               FSM_SIGNAL(MAIN, EV_CRANE_DONE, NULL);
+                               robot.sched.queue_event(robot.MAIN, new evCraneDone);
                        last_response = instance->response;
                        break;
                case DEADLINE:
@@ -429,7 +434,7 @@ void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
        switch (info->status) {
                case NEW_DATA:
                        if ((!last_strategy && instance->strategy) && (robot.start_state == POWER_ON)) {
-                                FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
+                                robot.sched.queue_event(robot.MAIN, new evSwitchStrategy());
                        }
                        last_strategy = instance->strategy;
                        break;
@@ -445,11 +450,11 @@ void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
         switch (info->status) {
                 case NEW_DATA:
                         if (!instance->bumper_rear) {
-                                FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+                                robot.sched.queue_event(robot.MOTION, new evObstacleBehind());
                         }
                         if ((!instance->bumper_left) ||
                             (!instance->bumper_right)) {
-                                FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
+                                robot.sched.queue_event(robot.MOTION, new evObstacleSide());
                         }
                         break;
                 case DEADLINE:

diff --git a/src/robofsm/robot_orte.h b/src/robofsm/robot_orte.h
index 14ff1d36161f4b2b2a5813efcd2a511d37ac98c3..fb4c64ddcda449c698ad247918f2d888afb6bc4c 100644 (file)
--- a/src/robofsm/robot_orte.h
+++ b/src/robofsm/robot_orte.h
@@ -18,12 +18,6 @@
 #ifndef ROBOT_ORTE_H
 #define ROBOT_ORTE_H
 
-#ifdef __cplusplus
-extern "C" {
-#endif 
-int __attribute ((warn_unused_result)) robot_init_orte();
-#ifdef __cplusplus
-}
-#endif 
+int robot_init_orte() __attribute__ ((warn_unused_result));
 
 #endif /* ROBOT_ORTE_H */