robofsm: motion IRC cb edit + ultrasonic dist measurement event

adapted to use with fsmloc & ULoPoS estimator

diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index 6d587b6aa4a67eeb327d633cb31f3dae28b13fc0..326c87a730294ca90430df30420c9552924dab5e 100644 (file)
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -15,8 +15,9 @@ events = {
        "EV_ENEMY_AHEAD" : "",
     },
     "loc" : {
-       "EV_ODO_RECEIVED" : "Execute prediction step based on odometry data",
-       "EV_LASER_RECEIVED" : "Execute update step based on laser measurement",
+       #"EV_ODO_RECEIVED" : "Execute prediction step based on odometry data",
+       #"EV_LASER_RECEIVED" : "Execute update step based on laser measurement",
+       "EV_DIST_RECEIVED" : "Ultrasound period: execute estimation & control step",
        "EV_SET_ESTIMATED" : "Sets the estimated position to the value from event parameter",
        "EV_RESET" : "Initialize MCL to uniform distribution on the whole playground",
     },

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 2e657304f1860e7dc2b0cce81c594a2b06a9410c..00b02383e4b70d4c047d207f002512086dbac57b 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -54,6 +54,7 @@ enum robot_state {
 #define ROBOT_FSM_MOTION motion
 #define ROBOT_FSM_DISPLAY display
 #define ROBOT_FSM_ACT act
+#define ROBOT_FSM_LOC loc
 
 #define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id)
 
@@ -113,6 +114,8 @@ extern struct lock_log robot_lock_log;
 #define __robot_lock_drives            lock
 #define __robot_lock_disp              lock_disp
 #define __robot_lock_state             lock
+#define __robot_lock_motion_irc         lock
+#define __robot_lock_corr_distances     lock
 
 /* robot's main data structure */
 struct robot {
@@ -145,17 +148,21 @@ struct robot {
                struct robo_fsm motion;
                struct robo_fsm display;
                struct robo_fsm act;
+               struct robo_fsm loc;
        } fsm;
 
        /* actual position */
        struct ref_pos_type ref_pos;
        /* estimated position */
        struct est_pos_type est_pos;
-
+       
        /* orte */
        struct robottype_orte_data orte;
 
        /* sensors */
+       struct motion_irc_type motion_irc;     /* odometry */
+       struct corr_distances_type corr_distances;  /* ultrasound */
+
        struct hokuyo_scan_type hokuyo;
        struct cmu_type cmu;
 

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index a8e36d37c3555877f819ecedeabb81dba02f00a2..8327a9b781ac8fb0035649f280718674c8acc6ff 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -58,38 +58,14 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
        struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
-       static struct motion_irc_type prevInstance;
-       static int firstRun = 1;
-       /* spocitat prevodovy pomer */
-       double n = (double)(28.0 / 1.0); 
-       /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
-       double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-        
-       double aktk0 = 0;
-       double aktk1 = 0;
-       double deltaU = 0;
-       double deltAlfa = 0;
 
        switch (info->status) {
                case NEW_DATA:
-                       if(firstRun) {
-                               prevInstance = *instance;
-                               firstRun = 0;
-                               break;
-                       }
-                       
-                       aktk0 = ((prevInstance.left - instance->left) >> 8) * c;
-                       aktk1 = ((instance->right - prevInstance.right) >> 8) * c;
-                       prevInstance = *instance;
-
-                       deltaU = (aktk0 + aktk1) / 2;
-                       deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
-
-                       struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
-                       memset(odo, 0, sizeof(*odo));
-                       odo->dx = deltaU;
-                       odo->dy = 0;
-                       odo->dangle = deltAlfa;
+                       /* locking should not be needed, but... */
+                       ROBOT_LOCK(motion_irc);
+                       robot.motion_irc = *instance;
+                       ROBOT_UNLOCK(motion_irc);
+
                        /* FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo); */
                        robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
                        break;
@@ -100,6 +76,31 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_corr_distances_cb(const ORTERecvInfo *info, void *vinstance,
+                          void *recvCallBackParam)
+{
+       struct corr_distances_type *instance =
+               (struct corr_distances_type *)vinstance;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       /* locking should not be needed, but... */
+                       ROBOT_LOCK(corr_distances);
+                       robot.corr_distances = *instance;
+                       ROBOT_UNLOCK(corr_distances);
+
+                       FSM_SIGNAL(LOC, EV_DIST_RECEIVED, NULL);
+
+                       /*FIXME: is following OK? (pecam1) */
+                       robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
+                       break;
+               case DEADLINE:
+                       robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
+                       DBG("ORTE deadline occurred - corr_distances receive\n");
+                       break;
+       }
+}
+
 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
@@ -341,6 +342,7 @@ int robot_init_orte()
        robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
        robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
        robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
+       robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
        robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
        robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
        robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);