"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",
},
#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)
#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 {
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;
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;
}
}
+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)
{
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);