X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/c188718736ad604b157be14a6acba5e6f05f912f..bfbd61a139592e6564fc79035e7d72e5668a002b:/src/robofsm/robot_orte.c diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 122bbdbf..9d5a2317 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -1,28 +1,38 @@ +/** + * @file robot_orte.c + * @date 2008-2010 + * + * @brief Module providing communication through ORTE + */ + /* * robot_orte.c 08/04/21 * * Robot's orte stuffs. * - * Copyright: (c) 2008 CTU Dragons + * Copyright: (c) 2008-2010 CTU Dragons * CTU FEE - Department of Control Engineering * License: GNU GPL v.2 */ #include #include -#include +#include "robodata.h" #include #include #include #include -#include #include "map_handling.h" -#include +#include "match-timing.h" #include #include +#include +#include + +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 @@ -44,24 +54,32 @@ void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance, ROBOT_UNLOCK(ref_pos); } -void send_est_pos_uzv_cb(const ORTESendInfo *info, void *vinstance, +void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance, void *sendCallBackParam) { struct robot_pos_type *instance = (struct robot_pos_type *)vinstance; - ROBOT_LOCK(est_pos_uzv); - *instance = robot.est_pos_uzv; - ROBOT_UNLOCK(est_pos_uzv); + ROBOT_LOCK(est_pos_odo); + *instance = robot.est_pos_odo; + ROBOT_UNLOCK(est_pos_odo); } -void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance, +void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance, void *sendCallBackParam) { struct robot_pos_type *instance = (struct robot_pos_type *)vinstance; - ROBOT_LOCK(est_pos_odo); - *instance = robot.est_pos_odo; - ROBOT_UNLOCK(est_pos_odo); + 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, + void *sendCallBackParam) +{ + struct robot_pos_type *instance = (struct robot_pos_type *)vinstance; + + robot_get_est_pos(&instance->x, &instance->y, &instance->phi); } void send_dummy_cb(const ORTESendInfo *info, void *vinstance, @@ -69,9 +87,75 @@ void send_dummy_cb(const ORTESendInfo *info, void *vinstance, { } +void send_match_time_cb(const ORTESendInfo *info, void *vinstance, + void *sendCallBackParam) +{ + struct match_time_type *instance = (struct match_time_type *)vinstance; + + if (robot.start_state == POWER_ON || robot.start_state == START_PLUGGED_IN) { + instance->time = 90; + } else { + instance->time = 90 - robot_current_time(); + } +} /* ---------------------------------------------------------------------- * SUBSCRIBER CALLBACKS - GENERIC * ---------------------------------------------------------------------- */ +void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance, + void *recvCallBackParam) +{ + struct odo_data_type *instance = (struct odo_data_type *)vinstance; + double dleft, dright, dtang, dphi; + static bool first_run = true; + /* spocitat prevodovy pomer */ + const double n = (double)(1.0 / 1.0); + + /* vzdalenost na pulz IRC */ + const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0); + + switch (info->status) { + case NEW_DATA: + if (first_run) { + ROBOT_LOCK(odo_data); + robot.odo_data = *instance; + ROBOT_UNLOCK(odo_data); + first_run = false; + break; + } + + dleft = ((robot.odo_data.left - instance->left) >> 8) * c * robot.odo_cal_a; // TODO >> 8 ? + dright = ((instance->right - robot.odo_data.right) >> 8) * c * robot.odo_cal_b; + + dtang = (dleft + dright) / 2.0; + dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M); + + ROBOT_LOCK(est_pos_indep_odo); + robot.odo_distance_a +=dleft; + robot.odo_distance_b +=dright; + 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); + + robot.indep_odometry_works = true; + + /* wake up motion-control/thread_trajectory_follower */ + sem_post(&measurement_received); + + //robot.hw_status[COMPONENT_ODO] = STATUS_OK; + break; + case DEADLINE: + robot.indep_odometry_works = false; + //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED; + DBG("ORTE deadline occurred - odo_data receive\n"); + break; + } +} void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) @@ -80,11 +164,10 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance, double dleft, dright, dtang, dphi; static bool first_run = true; /* spocitat prevodovy pomer */ - const double n = (double)(28.0 / 1.0); + const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0); /* vzdalenost na pulz IRC */ - const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0); - + const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION); switch (info->status) { case NEW_DATA: if (first_run) { @@ -95,9 +178,13 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance, break; } - dleft = ((robot.motion_irc.left - instance->left) >> 8) * c; - dright = ((instance->right - robot.motion_irc.right) >> 8) * c; + /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright), + what is the right solution? + This was neccessary to view motor odometry correctly in robomon. */ + dright = ((robot.motion_irc.left - instance->left) >> 8) * c; + dleft = ((instance->right - robot.motion_irc.right) >> 8) * c; + dtang = (dleft + dright) / 2.0; dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M); @@ -116,42 +203,16 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance, robot.odometry_works = true; - robot.hw_status[STATUS_MOTION] = HW_STATUS_OK; + robot.status[COMPONENT_MOTOR] = STATUS_OK; break; case DEADLINE: robot.odometry_works = false; - robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED; + robot.status[COMPONENT_MOTOR] = STATUS_FAILED; DBG("ORTE deadline occurred - motion_irc receive\n"); 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); - - /* wake up motion-control/thread_trajectory_follower */ - sem_post(&measurement_received); - - /*FIXME: is following OK? (pecam1) */ - robot.hw_status[STATUS_UZV] = HW_STATUS_OK; - break; - case DEADLINE: - robot.hw_status[STATUS_UZV] = HW_STATUS_FAILED; - - break; - } -} - void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { @@ -181,10 +242,10 @@ void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance, { switch (info->status) { case NEW_DATA: - robot.hw_status[STATUS_PWR]=HW_STATUS_OK; + robot.status[COMPONENT_POWER]=STATUS_OK; break; case DEADLINE: - robot.hw_status[STATUS_PWR]=HW_STATUS_FAILED; + robot.status[COMPONENT_POWER]=STATUS_FAILED; DBG("ORTE deadline occurred - pwr_voltage receive\n"); break; } @@ -212,34 +273,30 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, case NEW_DATA: /* Stupid way of controlling the robot by * pluggin in and out start connector. */ - switch (robot.state) { + switch (robot.start_state) { case POWER_ON: - if (instance->start) { - robot.hw_status[STATUS_STA] = HW_STATUS_WARNING; - } else { - robot.hw_status[STATUS_STA] = HW_STATUS_OK; - } - if (!instance->start) { - robot.state = START_PLUGGED_IN; - printf("START_PLUGGED_IN\n"); + if (!instance->start_condition) { + robot.status[COMPONENT_START] = STATUS_WARNING; + robot.start_state = START_PLUGGED_IN; + ul_logmsg("START_PLUGGED_IN\n"); } break; case START_PLUGGED_IN: - robot.hw_status[STATUS_STA] = HW_STATUS_OK; + robot.status[COMPONENT_START] = STATUS_OK; /* Competition starts when plugged out */ - if (instance->start) { + if (instance->start_condition) { FSM_SIGNAL(MAIN, EV_START, NULL); - robot.state = COMPETITION_STARTED; - printf("STARTED\n"); + robot.start_state = COMPETITION_STARTED; + ul_logmsg("STARTED\n"); } break; case COMPETITION_STARTED: { /* Subsequent plug-in stops the robot */ static int num = 0; - if (!instance->start) { - robot.hw_status[STATUS_STA] = HW_STATUS_WARNING; + if (!instance->start_condition) { + robot.status[COMPONENT_START] = STATUS_WARNING; if (++num == 10) robot_exit(); } @@ -249,7 +306,7 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, last_instance = *instance; break; case DEADLINE: - robot.hw_status[STATUS_STA] = HW_STATUS_FAILED; + robot.status[COMPONENT_START] = STATUS_FAILED; DBG("ORTE deadline occurred - robot_cmd receive\n"); break; } @@ -264,19 +321,23 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, case NEW_DATA: { ROBOT_LOCK(hokuyo); robot.hokuyo = *instance; - robot.hw_status[STATUS_HOK] = HW_STATUS_OK; + robot.status[COMPONENT_HOKUYO] = STATUS_OK; ROBOT_UNLOCK(hokuyo); - update_map_hokuyo(instance); + if(!robot.ignore_hokuyo) + { + update_map_hokuyo(instance); + } break; } case DEADLINE: - robot.hw_status[STATUS_HOK] = HW_STATUS_FAILED; + robot.status[COMPONENT_HOKUYO] = STATUS_FAILED; //system("killall -9 hokuyo"); DBG("%s: ORTE deadline occurred\n", __FUNCTION__); break; } } +// FIXME: this is not up to date (Filip, 2010) void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { @@ -285,14 +346,15 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, switch (info->status) { case NEW_DATA: { ROBOT_LOCK(camera_result); - robot.game_conf = instance->lot; + robot.corns_conf_side = instance->side; + robot.corns_conf_center = instance->center; ROBOT_UNLOCK(camera_result); - robot.hw_status[STATUS_CAM] = HW_STATUS_OK; + robot.status[COMPONENT_CAMERA] = STATUS_OK; break; } case DEADLINE: if (robot.orte.camera_control.on) { - robot.hw_status[STATUS_CAM] = HW_STATUS_FAILED; + robot.status[COMPONENT_CAMERA] = STATUS_FAILED; //system("killall -9 rozpuk"); } @@ -304,104 +366,98 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, * SUBSCRIBER CALLBACKS - EB2008 * ---------------------------------------------------------------------- */ -void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance, +void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { - static unsigned short old_lift_pos=0; - static unsigned short old_pusher_pos=0; + struct jaws_status_type *instance = (struct jaws_status_type *)vinstance; + static int last_response_left = 0; + static int last_response_right = 0; switch (info->status) { case NEW_DATA: // new data arrived and requested position equals actual position - ROBOT_LOCK(lift_cmd); - if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos && - robot.lift_cmd > 0) { - FSM_SIGNAL(ACT, EV_LIFT_IN_POS, NULL); - robot.lift_cmd--; - } - ROBOT_UNLOCK(lift_cmd); - if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos && - robot.orte.actuator_status.pusher_pos != old_pusher_pos) - FSM_SIGNAL(ACT, EV_PUSHER_IN_POS, NULL); - if (robot.orte.actuator_status.status & ~LIFT_ENDSW_MASK) { - robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING; - } else { - robot.hw_status[STATUS_LFT]=HW_STATUS_OK; - } - old_lift_pos = robot.orte.actuator_status.lift_pos; - old_pusher_pos = robot.orte.actuator_status.pusher_pos; + if (instance->flags.left == 0 && + instance->flags.right == 0) + robot.status[COMPONENT_JAWS] = STATUS_OK; + else + robot.status[COMPONENT_JAWS] = STATUS_WARNING; + + if (instance->response.left != last_response_left && + instance->response.right != last_response_right && + instance->response.left == act_jaw_left_get_last_reqest() && + instance->response.left == act_jaw_right_get_last_reqest()) + FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL); + + last_response_left = instance->response.left; + last_response_right = instance->response.right; break; case DEADLINE: - robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED; + robot.status[COMPONENT_JAWS] = STATUS_FAILED; DBG("ORTE deadline occurred - actuator_status receive\n"); break; } } -void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) +void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance, + void *recvCallBackParam) { - switch (info->status) { - case NEW_DATA: { - double old_distance_value = robot.puck_distance; + struct lift_status_type *instance = (struct lift_status_type *)vinstance; + static int last_response = 0; + switch (info->status) { + case NEW_DATA: + // new data arrived and requested position equals actual position + if (instance->flags == 0) + robot.status[COMPONENT_LIFT] = STATUS_OK; + else + robot.status[COMPONENT_LIFT] = STATUS_WARNING; + + if (instance->response != last_response && + instance->response == act_lift_get_last_reqest()) + FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL); + last_response = instance->response; + break; + case DEADLINE: + robot.status[COMPONENT_LIFT] = STATUS_FAILED; + DBG("ORTE deadline occurred - actuator_status receive\n"); + break; + } +} - ROBOT_LOCK(sharps); - robot.puck_distance = robot.orte.puck_distance.distance; - ROBOT_UNLOCK(sharps); +void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, + void *recvCallBackParam) +{ + struct robot_switches_type *instance = (struct robot_switches_type *)vinstance; + static bool last_strategy; + switch (info->status) { + case NEW_DATA: + robot.team_color = instance->team_color; - if (PUCK_INSIDE(robot.orte.puck_distance.distance) && !PUCK_INSIDE(old_distance_value)) { - // signal puck is inside - FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL); - } - if (PUCK_REACHABLE(robot.orte.puck_distance.distance) && !PUCK_REACHABLE(old_distance_value)) { - // signal puck is reachable - FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL); + if (!last_strategy && instance->strategy) { + /* strategy switching */ + FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL); } - } - break; - case DEADLINE: - // FIXME F.J. (?) inform display in case of invalid data? - // FIXME F.J. (?) limited space on the display... - //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED; - DBG("ORTE deadline occurred - servos receive\n"); - break; + last_strategy = instance->strategy; + break; + case DEADLINE: + break; } } -void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) +void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance, + void *recvCallBackParam) { + struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance; + static bool last_left, last_right; switch (info->status) { - case NEW_DATA: { - if (robot.use_back_switch) { - static bool previous_switch_status = 0; - if (robot.orte.puck_inside.status & 0x02) { - //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL); - - /* React on the first switch or when moving backward. */ - if ((previous_switch_status & 0x02) == 0 || - robot.moves_backward) { - FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL); - } - previous_switch_status = robot.orte.puck_inside.status; - } - } - { - static bool previous = false; - bool switched = ((robot.orte.puck_inside.status & 0x01) != 0); - - if (switched && !previous) { - FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL); + case NEW_DATA: + if (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right) + FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL); + + if (instance->bumper_left || instance->bumper_right) { + FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL); } - previous = switched; - - } - robot.hw_status[STATUS_CHE]=HW_STATUS_OK; - } - break; - case DEADLINE: - robot.hw_status[STATUS_CHE]=HW_STATUS_FAILED; - DBG("ORTE deadline occurred - servos receive\n"); - break; + break; + case DEADLINE: + break; } } @@ -432,8 +488,10 @@ int robot_init_orte() /* creating publishers */ robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte); robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte); - robottype_publisher_est_pos_uzv_create(&robot.orte, send_est_pos_uzv_cb, &robot.orte); robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte); + robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte); + robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte); + robottype_publisher_match_time_create(&robot.orte, send_match_time_cb, &robot.orte); //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte); // I didn't know what was the difference between the callback function pointer @@ -441,30 +499,26 @@ int robot_init_orte() // IS CRUCIAL! // - NULL: message is published only when OrtePublicationSend called // - pointer to empty function: message is published periodically - robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte); - robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte); - robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte); - robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte); - robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte); robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte); robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte); robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte); robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte); + robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte); + robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte); /* create generic subscribers */ + robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.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); robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte); - - /* create subscribers */ - robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte); - //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte); - robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte); + robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte); + robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte); + robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte); + robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte); robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte); return rv;