From 2263832d44312b403d0bcf935f464d02130a104c Mon Sep 17 00:00:00 2001 From: Petr Silhavik Date: Wed, 5 Dec 2012 19:05:03 +0100 Subject: [PATCH] Transform to C++: Remove old c files --- src/robofsm/actuators.c | 97 -------- src/robofsm/robot.c | 269 -------------------- src/robofsm/robot_orte.c | 526 --------------------------------------- 3 files changed, 892 deletions(-) delete mode 100644 src/robofsm/actuators.c delete mode 100644 src/robofsm/robot.c delete mode 100644 src/robofsm/robot_orte.c diff --git a/src/robofsm/actuators.c b/src/robofsm/actuators.c deleted file mode 100644 index 33ac0b11..00000000 --- a/src/robofsm/actuators.c +++ /dev/null @@ -1,97 +0,0 @@ -/** - * @file actuators.c - * @author Martin Zidek - * @author Michal Sojka - * @author Filip Jares (?) - * @date 2009-2010 - * - * @brief Robot's actuators control library - */ - -/* - * actuators.c 09/02/25 - * - * Robot's actuators control. - * - * Copyright: (c) 2008-2010 CTU Dragons - * CTU FEE - Department of Control Engineering - * License: GNU GPL v.2 - */ - -#include -#include - -static struct robottype_orte_data *orte = NULL; - -static uint16_t jaw_left_last_request; -static uint16_t jaw_right_last_request; -static uint16_t lift_last_request; - -void act_init(struct robottype_orte_data *ortedata) -{ - orte = ortedata; - act_jaws(OPEN); - act_lift(0, 0, 1); -} - -void act_camera_on(void) -{ - orte->camera_control.on = 1; - ORTEPublicationSend(orte->publication_camera_control); -} - -void act_camera_off(void) -{ - orte->camera_control.on = 0; - ORTEPublicationSend(orte->publication_camera_control); -} - -void act_lift(uint16_t req_pos, char speed, char homing) -{ - - orte->lift_cmd.req_pos = req_pos; - orte->lift_cmd.speed = speed; - orte->lift_cmd.homing = homing; - /* Remember the request so that we change check for matching - * response in rcv_vidle_status_cb() */ - lift_last_request = req_pos; - ORTEPublicationSend(orte->publication_lift_cmd); -} - -void act_jaws(jaws_cmds cmd) -{ - switch (cmd) { - case OPEN: - orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN; - usleep(300000); - orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN; - break; - case CLOSE: - orte->jaws_cmd.req_pos.left = JAW_LEFT_CLOSE; - usleep(300000); - orte->jaws_cmd.req_pos.right = JAW_RIGHT_CLOSE; - break; - case CATCH: - orte->jaws_cmd.req_pos.left = JAW_LEFT_CATCH; - orte->jaws_cmd.req_pos.right = JAW_RIGHT_CATCH; - break; - default: - orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN; - orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN; - } -} - -uint16_t act_jaw_left_get_last_reqest(void) -{ - return jaw_left_last_request; -} - -uint16_t act_jaw_right_get_last_reqest(void) -{ - return jaw_right_last_request; -} - -uint16_t act_lift_get_last_reqest(void) -{ - return lift_last_request; -} diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c deleted file mode 100644 index ad7cbb75..00000000 --- a/src/robofsm/robot.c +++ /dev/null @@ -1,269 +0,0 @@ -/* - * robot_demo2011.c - * - * Robot's generic initialization and clean up functions. - * - * Copyright: (c) 2008 CTU Dragons - * CTU FEE - Department of Control Engineering - * License: GNU GPL v.2 - */ - -#define _XOPEN_SOURCE 500 /* For PTHREAD_PRIO_INHERIT etc. */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "map_handling.h" -#include -#include "actuators.h" -#include -#include - -UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */ - - -#define MOTION_CONTROL_INIT_ONLY -#include "motion-control.h" -#include "robot.h" - -/* Global definition of robot structure */ -struct robot robot; - -#ifdef CONFIG_LOCK_CHECKING -struct lock_log robot_lock_log; -#endif - -static void block_signals() -{ - sigset_t sigset; - int i; - - sigemptyset(&sigset); - for (i=SIGRTMIN; i<=SIGRTMAX; i++) - sigaddset(&sigset, i); - - pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL); -} - -static void int_handler(int sig) -{ - robot_exit(); -} - -void fill_in_known_areas_in_map() -{ - /* No known obstacles */ -} - -static void trans_callback(struct robo_fsm *fsm) -{ - if (fsm == &robot.fsm.main) { - strncpy(robot.orte.fsm_main.state_name, fsm->state_name, sizeof(robot.orte.fsm_main.state_name)); - ORTEPublicationSend(robot.orte.publication_fsm_main); - } else if (fsm == &robot.fsm.motion) { - strncpy(robot.orte.fsm_motion.state_name, fsm->state_name, sizeof(robot.orte.fsm_motion.state_name)); - ORTEPublicationSend(robot.orte.publication_fsm_motion); - } else if (fsm == &robot.fsm.act) { - strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name)); - ORTEPublicationSend(robot.orte.publication_fsm_act); - } - -} - -/** - * Initializes the robot. - * Setup fields in robot structure, initializes FSMs and ORTE. - * - * @return 0 - */ -int robot_init() -{ - int rv; - pthread_mutexattr_t mattr; - rv = pthread_mutexattr_init(&mattr); -#ifdef HAVE_PRIO_INHERIT - rv = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT); -#endif - pthread_mutex_init(&robot.lock, &mattr); - pthread_mutex_init(&robot.lock_ref_pos, &mattr); - pthread_mutex_init(&robot.lock_est_pos_odo, &mattr); - pthread_mutex_init(&robot.lock_est_pos_indep_odo, &mattr); - pthread_mutex_init(&robot.lock_meas_angles, &mattr); - pthread_mutex_init(&robot.lock_joy_data, &mattr); - pthread_mutex_init(&robot.lock_disp, &mattr); - - fsm_main_loop_init(&robot.main_loop); - - /* FSM initialization */ - fsm_init(&robot.fsm.main, "MAIN", &robot.main_loop); - fsm_init(&robot.fsm.motion, "\tmot", &robot.main_loop); - fsm_init(&robot.fsm.act, "\tACT", &robot.main_loop); - robot.fsm.main.transition_callback = trans_callback; - robot.fsm.act.transition_callback = trans_callback; - robot.fsm.motion.transition_callback = trans_callback; - - robot.team_color = VIOLET; - - if (robot.team_color == VIOLET) { - ul_loginf("We are VIOLET!\n"); - } else { - ul_loginf("We are RED!\n"); - } - - robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG)); - - robot.ignore_hokuyo = false; - robot.map = ShmapInit(1); - fill_in_known_areas_in_map(); - - signal(SIGINT, int_handler); - signal(SIGTERM, int_handler); - block_signals(); - - /* initial values */ - robot.orte.motion_speed.left = 0; - robot.orte.motion_speed.right = 0; - - robot.orte.pwr_ctrl.voltage33 = 1; - robot.orte.pwr_ctrl.voltage50 = 1; - robot.orte.pwr_ctrl.voltage80 = 1; - - robot.orte.camera_control.on = true; - - robot.fsm.motion.state = &fsm_state_motion_init; - - /* Only activate display if it is configured */ - /* FIXME: display - robot.sercom = uoled_init(serial_comm); - if (strcmp(robot.sercom->devname, "/dev/null") != 0) - robot.fsm.display.state = &fsm_state_disp_init; - */ - - robot.obstacle_avoidance_enabled = true; - robot.use_back_bumpers = true; - robot.use_left_bumper = true; - robot.use_right_bumper = true; - robot.start_state = POWER_ON; - robot.check_turn_safety = true; - - /* init ORTE domain, create publishers, subscribers, .. */ - rv = robot_init_orte(); - act_init(&robot.orte); - - return rv; -} - -/** - * Starts the robot FSMs and threads. - * - * @return 0 - */ -int robot_start() -{ - int rv = 0; - - pthread_attr_t tattr; - struct sched_param param; - pthread_t thr_obstacle_forgeting; - int ret; - - ret = motion_control_init(); - if(ret) { - perror("motion_control_init"); - robot_exit(); - } - - - /* Obstacle forgeting thread */ - pthread_attr_init (&tattr); - pthread_attr_getschedparam (&tattr, ¶m); - pthread_attr_getschedparam (&tattr, ¶m); - pthread_attr_setschedpolicy(&tattr, SCHED_FIFO); - param.sched_priority = OBST_FORGETING_PRIO; - rv = pthread_attr_setschedparam (&tattr, ¶m); - if (rv) { - perror("robot_start: pthread_attr_setschedparam()"); - goto err; - } - rv = pthread_create(&thr_obstacle_forgeting, - &tattr, thread_obstacle_forgeting, NULL); - if (rv) { - perror("robot_start: pthread_create"); - goto err; - } - - sem_init(&robot.start, 0, 0); - - fsm_main_loop(&robot.main_loop); - -err: - return rv; -} - -/** - * Signals all the robot threads to finish. - */ -void robot_exit() -{ - /* stop FSMs */ - fsm_exit(&robot.fsm.main); - fsm_exit(&robot.fsm.motion); - fsm_exit(&robot.fsm.act); -} - -/** - * Stops the robot. All resources alocated by robot_init() or - * robot_start() are dealocated here. - */ -void robot_destroy() -{ - motion_control_done(); - - // FIXME: set actuators to well defined position (FJ) - - robottype_roboorte_destroy(&robot.orte); - - fsm_destroy(&robot.fsm.main); - fsm_destroy(&robot.fsm.motion); - fsm_destroy(&robot.fsm.act); - ShmapFree(); - ul_logdeb("robofsm: stop.\n"); -} - -void robot_get_est_pos_trans(double *x, double *y, double *phi) -{ - robot_get_est_pos(x, y, phi); - *x = __trans_x(*x); - *y = __trans_y(*y); - *phi = __trans_ang(*phi); -} - -void robot_get_est_pos(double *x, double *y, double *phi) -{ - if (robot.indep_odometry_works) { - ROBOT_LOCK(est_pos_indep_odo); - *x = robot.est_pos_indep_odo.x; - *y = robot.est_pos_indep_odo.y; - *phi = robot.est_pos_indep_odo.phi; - ROBOT_UNLOCK(est_pos_indep_odo); - } else if (robot.odometry_works) { - ROBOT_LOCK(est_pos_odo); - *x = robot.est_pos_odo.x; - *y = robot.est_pos_odo.y; - *phi = robot.est_pos_odo.phi; - ROBOT_UNLOCK(est_pos_odo); - } else { - ROBOT_LOCK(ref_pos); - *x = robot.ref_pos.x; - *y = robot.ref_pos.y; - *phi = robot.ref_pos.phi; - ROBOT_UNLOCK(ref_pos); - } -} diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c deleted file mode 100644 index 9d5a2317..00000000 --- a/src/robofsm/robot_orte.c +++ /dev/null @@ -1,526 +0,0 @@ -/** - * @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-2010 CTU Dragons - * CTU FEE - Department of Control Engineering - * License: GNU GPL v.2 - */ - -#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, ...) ul_logdeb(format, ##__VA_ARGS__) -#else - #define DBG(format, ...) -#endif - -/*****FIXME:*****/ -extern sem_t measurement_received; - -/* ---------------------------------------------------------------------- - * PUBLISHER CALLBACKS - GENERIC - * ---------------------------------------------------------------------- */ - -void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance, - void *sendCallBackParam) -{ - struct robot_pos_type *instance = (struct robot_pos_type *)vinstance; - - ROBOT_LOCK(ref_pos); - *instance = robot.ref_pos; - ROBOT_UNLOCK(ref_pos); -} - -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_odo); - *instance = robot.est_pos_odo; - ROBOT_UNLOCK(est_pos_odo); -} - -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_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, - void *sendCallBackParam) -{ -} - -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) -{ - struct motion_irc_type *instance = (struct motion_irc_type *)vinstance; - double dleft, dright, dtang, dphi; - static bool first_run = true; - /* spocitat prevodovy pomer */ - 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*ROBOT_MOTOR_IRC_RESOLUTION); - switch (info->status) { - case NEW_DATA: - if (first_run) { - ROBOT_LOCK(motion_irc); - robot.motion_irc = *instance; - ROBOT_UNLOCK(motion_irc); - first_run = false; - break; - } - - - /* 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); - - 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); - - /* locking should not be needed, but... */ - ROBOT_LOCK(motion_irc); - robot.motion_irc = *instance; - robot.motion_irc_received = 1; - ROBOT_UNLOCK(motion_irc); - - robot.odometry_works = true; - - robot.status[COMPONENT_MOTOR] = STATUS_OK; - break; - case DEADLINE: - robot.odometry_works = false; - robot.status[COMPONENT_MOTOR] = STATUS_FAILED; - DBG("ORTE deadline occurred - motion_irc receive\n"); - break; - } -} - -void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - switch (info->status) { - case NEW_DATA: - break; - case DEADLINE: - DBG("ORTE deadline occurred - motion_speed receive\n"); - break; - } -} - -void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - switch (info->status) { - case NEW_DATA: - break; - case DEADLINE: - DBG("ORTE deadline occurred - motion_status receive\n"); - break; - } -} - -void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - switch (info->status) { - case NEW_DATA: - robot.status[COMPONENT_POWER]=STATUS_OK; - break; - case DEADLINE: - robot.status[COMPONENT_POWER]=STATUS_FAILED; - DBG("ORTE deadline occurred - pwr_voltage receive\n"); - break; - } -} - -void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - switch (info->status) { - case NEW_DATA: - break; - case DEADLINE: - DBG("ORTE deadline occurred - pwr_ctrl receive\n"); - break; - } -} - -void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance; - static struct robot_cmd_type last_instance; - - switch (info->status) { - case NEW_DATA: - /* Stupid way of controlling the robot by - * pluggin in and out start connector. */ - switch (robot.start_state) { - case POWER_ON: - 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.status[COMPONENT_START] = STATUS_OK; - /* Competition starts when plugged out */ - if (instance->start_condition) { - FSM_SIGNAL(MAIN, EV_START, NULL); - 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_condition) { - robot.status[COMPONENT_START] = STATUS_WARNING; - if (++num == 10) - robot_exit(); - } - break; - } - } - last_instance = *instance; - break; - case DEADLINE: - robot.status[COMPONENT_START] = STATUS_FAILED; - DBG("ORTE deadline occurred - robot_cmd receive\n"); - break; - } -} - -void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance; - - switch (info->status) { - case NEW_DATA: { - ROBOT_LOCK(hokuyo); - robot.hokuyo = *instance; - robot.status[COMPONENT_HOKUYO] = STATUS_OK; - ROBOT_UNLOCK(hokuyo); - if(!robot.ignore_hokuyo) - { - update_map_hokuyo(instance); - } - break; - } - case DEADLINE: - 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) -{ - struct camera_result_type *instance = (struct camera_result_type *)vinstance; - - switch (info->status) { - case NEW_DATA: { - ROBOT_LOCK(camera_result); - robot.corns_conf_side = instance->side; - robot.corns_conf_center = instance->center; - ROBOT_UNLOCK(camera_result); - robot.status[COMPONENT_CAMERA] = STATUS_OK; - break; - } - case DEADLINE: - if (robot.orte.camera_control.on) { - robot.status[COMPONENT_CAMERA] = STATUS_FAILED; - //system("killall -9 rozpuk"); - } - - DBG("%s: ORTE deadline occurred\n", __FUNCTION__); - break; - } -} -/* ---------------------------------------------------------------------- - * SUBSCRIBER CALLBACKS - EB2008 - * ---------------------------------------------------------------------- */ - -void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - 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.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.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_strategy; - switch (info->status) { - case NEW_DATA: - robot.team_color = instance->team_color; - - 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 (instance->bumper_left || instance->bumper_right) { - FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL); - } - break; - case DEADLINE: - break; - } -} - -#define HIST_CNT 5 -#if 0 -static int cmp_double(const void *v1, const void *v2) -{ - const double *d1 = v1, *const d2 = v2; - if (d1 < d2) - return -1; - else if (d1 > d2) - return +1; - else - return 0; -} -#endif - -int robot_init_orte() -{ - int rv = 0; - - robot.orte.strength = 20; - - rv = robottype_roboorte_init(&robot.orte); - if (rv) - return rv; - - /* 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_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 - // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE - // IS CRUCIAL! - // - NULL: message is published only when OrtePublicationSend called - // - pointer to empty function: message is published periodically - 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_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); - 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; -} - -- 2.39.2