X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/8fa7e4218eeaccc37035791440101112727f3354..9de11695d9f12221e00398aa4e5481d2dcb80580:/src/robofsm/robot_orte.c diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 1676ca57..02475186 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -17,18 +17,22 @@ #include #include -#include +#include "robodata.h" #include #include #include #include #include "map_handling.h" +#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 @@ -83,6 +87,17 @@ 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 * ---------------------------------------------------------------------- */ @@ -108,13 +123,15 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance, break; } - dleft = ((robot.odo_data.left - instance->left) >> 8) * c; // TODO >> 8 ? - dright = ((instance->right - robot.odo_data.right) >> 8) * c; + 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); @@ -147,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) { @@ -162,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 * robot.odo_cal_b; + dleft = ((instance->right - robot.motion_irc.right) >> 8) * c * robot.odo_cal_a; + dtang = (dleft + dright) / 2.0; dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M); @@ -258,7 +278,7 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, 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 +288,7 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, if (instance->start_condition) { FSM_SIGNAL(MAIN, EV_START, NULL); robot.start_state = COMPETITION_STARTED; - printf("STARTED\n"); + ul_logmsg("STARTED\n"); } break; @@ -346,50 +366,95 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, * SUBSCRIBER CALLBACKS - EB2008 * ---------------------------------------------------------------------- */ -void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance, +void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { - struct vidle_status_type *instance = (struct vidle_status_type *)vinstance; - static int last_response = 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 - if (instance->flags == 0) - robot.status[COMPONENT_VIDLE]=STATUS_OK; + if (instance->flags.left == 0 && + instance->flags.right == 0) + robot.status[COMPONENT_JAWS] = STATUS_OK; else - robot.status[COMPONENT_VIDLE]=STATUS_WARNING; + 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); - if (instance->response != last_response) - FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL); - last_response = instance->response; + last_response_left = instance->response.left; + last_response_right = instance->response.right; break; case DEADLINE: - robot.status[COMPONENT_VIDLE]=STATUS_FAILED; + robot.status[COMPONENT_JAWS] = STATUS_FAILED; DBG("ORTE deadline occurred - actuator_status receive\n"); break; } } +void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance, + void *recvCallBackParam) +{ + 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; + } +} + 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_left, last_right; + static bool last_strategy; switch (info->status) { case NEW_DATA: robot.team_color = instance->team_color; - if (instance->bumper_pressed) + + if (!last_strategy && instance->strategy) { + /* strategy switching */ + FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL); + } + last_strategy = instance->strategy; + break; + case DEADLINE: + break; + } +} + +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 (instance->bumper_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right) FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL); - if ((!last_left && instance->bumper_left) || - (!last_right && instance->bumper_right)) { + + if (instance->bumper_left || instance->bumper_right) { FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL); - if (robot.start_state == POWER_ON) { - /* Reuse VIDLE done for strategy switching */ - FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL); - } } - last_right = instance->bumper_right; - last_left = instance->bumper_left; break; case DEADLINE: break; @@ -426,6 +491,7 @@ int robot_init_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 @@ -437,7 +503,8 @@ int robot_init_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_vidle_cmd_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); @@ -448,9 +515,11 @@ int robot_init_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); - robottype_subscriber_vidle_status_create(&robot.orte, rcv_vidle_status_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_camera_result_create(&robot.orte, rcv_camera_result_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; }