From d587fbfd82b3669dcda0ea26b29a9f881c4db0c1 Mon Sep 17 00:00:00 2001 From: Petr Silhavik Date: Sun, 17 Feb 2013 17:55:25 +0100 Subject: [PATCH] robofsm: Robot is now class with functions --- src/robofsm/robot.cc | 115 +++++++++++++++++++------------------ src/robofsm/robot.h | 131 +++++++++++-------------------------------- 2 files changed, 94 insertions(+), 152 deletions(-) diff --git a/src/robofsm/robot.cc b/src/robofsm/robot.cc index 3e977dd1..8edaf2df 100644 --- a/src/robofsm/robot.cc +++ b/src/robofsm/robot.cc @@ -11,7 +11,6 @@ #define _XOPEN_SOURCE 500 /* For PTHREAD_PRIO_INHERIT etc. */ #include -#include #include #include #include @@ -25,19 +24,17 @@ #include "actuators.h" #include #include +#include "guard.hpp" +#include "fsmmove.cc" -UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */ +//UL_LOG_CUST(ulogd_robot); /* Log domain name = ulogd + name of the file */ #define MOTION_CONTROL_INIT_ONLY #include "motion-control.h" /* Global definition of robot structure */ -struct robot robot; - -#ifdef CONFIG_LOCK_CHECKING -struct lock_log robot_lock_log; -#endif +Robot robot; static void block_signals() { @@ -53,7 +50,7 @@ static void block_signals() static void int_handler(int sig) { - robot_exit(); + robot.robot_exit(); } void fill_in_known_areas_in_map() @@ -67,7 +64,7 @@ void fill_in_known_areas_in_map() static void trans_callback(struct robo_fsm *fsm) { - if (fsm == &robot.fsm.main) { +/* 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) { @@ -77,7 +74,7 @@ static void trans_callback(struct robo_fsm *fsm) strncpy(robot.orte.fsm_act.state_name, fsm->state_name, sizeof(robot.orte.fsm_act.state_name)); ORTEPublicationSend(robot.orte.publication_fsm_act); } - +*/ } /** @@ -86,7 +83,7 @@ static void trans_callback(struct robo_fsm *fsm) * * @return 0 */ -int robot_init() +int Robot::init() { int rv; pthread_mutexattr_t mattr; @@ -103,19 +100,11 @@ int robot_init() 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; + MOTION = sched.create_processor(); robot.team_color = BLUE; - robot_set_est_pos_trans(1, 1, DEG2RAD(0)); + set_est_pos_trans(1, 1, DEG2RAD(0)); robot.ignore_hokuyo = false; robot.map = ShmapInit(1); @@ -135,7 +124,6 @@ int robot_init() robot.orte.camera_control.on = false; - robot.fsm.motion.state = &fsm_state_motion_init; /* Only activate display if it is configured */ /* FIXME: display @@ -163,7 +151,7 @@ int robot_init() * * @return 0 */ -int robot_start() +int Robot::start() { int rv = 0; @@ -197,9 +185,6 @@ int robot_start() goto err; } - sem_init(&robot.start, 0, 0); - - fsm_main_loop(&robot.main_loop); err: return rv; @@ -211,16 +196,13 @@ err: 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() +void Robot::destroy() { motion_control_done(); @@ -228,40 +210,65 @@ void robot_destroy() 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) +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); + get_est_pos(x, y, phi); + x = trans_x(x); + y = trans_y(y); + phi = trans_angle(phi); } -void robot_get_est_pos(double *x, double *y, double *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); + Guard g(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; } 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); + Guard g(robot.lock_est_pos_odo); + x = robot.est_pos_odo.x; + y = robot.est_pos_odo.y; + phi = robot.est_pos_odo.phi; } else { - ROBOT_LOCK(ref_pos); - *x = robot.ref_pos.x; - *y = robot.ref_pos.y; - *phi = robot.ref_pos.phi; - ROBOT_UNLOCK(ref_pos); + Guard g(robot.lock_ref_pos); + x = robot.ref_pos.x; + y = robot.ref_pos.y; + phi = robot.ref_pos.phi; } } + +/** Sets actual position of the robot and with respoect to color of + * the team. Should be used for setting initial position of the + * robot. */ +void Robot::set_est_pos(double x, double y, double phi) { + if (x<0) x=0; + if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M; + if (y<0) y=0; + if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M; + + { + Guard g(lock_est_pos_indep_odo); + est_pos_indep_odo.x = x; + est_pos_indep_odo.y = y; + est_pos_indep_odo.phi = phi; + } + + { + Guard g(lock_est_pos_odo); + est_pos_odo.x = x; + est_pos_odo.y = y; + est_pos_odo.phi = phi; + } + + { + Guard g(lock_ref_pos); + ref_pos.x = x; + ref_pos.y = y; + ref_pos.phi = phi; + } +} \ No newline at end of file diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h index 4ff6ffee..d566862a 100644 --- a/src/robofsm/robot.h +++ b/src/robofsm/robot.h @@ -15,21 +15,23 @@ #include #include #include -#include +//#include #include #include #include -#include +//#include #include #include -//#include +#include +#include +#include //UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */ /** * Competition parameters */ -enum team_color { +enum team_colour { BLUE = 0, YELLOW }; @@ -46,66 +48,6 @@ enum team_color { #define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id) -/** - * LOCKING MANIPULATION - */ - -#ifdef CONFIG_LOCK_CHECKING -#define LOCK_CHECK_COUNT 10 -struct lock_log { - pthread_mutex_t *locked[LOCK_CHECK_COUNT]; - int idx; -}; - -extern struct lock_log robot_lock_log; - -#define __LOCK_CHECK(mutex) robot_lock_log.locked[robot_lock_log.idx++] = mutex; -#define __UNLOCK_CHECK(mutex) \ - if (robot_lock_log.locked[--robot_lock_log.idx] != mutex || \ - robot_lock_log.idx < 0) \ - ul_logerr("!!! Locking error %s:%d\n", __FILE__, __LINE__); -#else -#define __LOCK_CHECK(mutex) -#define __UNLOCK_CHECK(mutex) -#endif -/** - * Locks the robot structure. - * @param var Field in the structure you are going to work with. - */ -#define ROBOT_LOCK(var) \ - do { \ - pthread_mutex_lock(&robot.__robot_lock_##var); \ - __LOCK_CHECK(&robot.__robot_lock_##var); \ - } while(0) - -/** - * Unlocks the robot structure. - * @param var Field in the structure, which was locked by ROBOT_LOCK. - */ -#define ROBOT_UNLOCK(var) \ - do { \ - __UNLOCK_CHECK(&robot.__robot_lock_##var); \ - pthread_mutex_unlock(&robot.__robot_lock_##var); \ - } while(0) - -/* Mapping of robot structure fields to locks */ -//#define __robot_lock_ lock /* ROBOT_LOCK() */ -#define __robot_lock_ref_pos lock_ref_pos -#define __robot_lock_est_pos_odo lock_est_pos_odo -#define __robot_lock_est_pos_indep_odo lock_est_pos_indep_odo -#define __robot_lock_joy_data lock_joy_data -#define __robot_lock_meas_angles lock_meas_angles -#define __robot_lock_drives lock -#define __robot_lock_sharps lock -#define __robot_lock_hokuyo lock -#define __robot_lock_cmu lock -#define __robot_lock_bumper lock -#define __robot_lock_disp lock_disp -#define __robot_lock_motion_irc lock -#define __robot_lock_odo_data lock -#define __robot_lock_corr_distances lock -#define __robot_lock_camera_result lock_camera - enum robot_status { STATUS_OK, STATUS_WARNING, @@ -125,7 +67,9 @@ enum robot_component { }; /* robot's main data structure */ -struct robot { +class Robot { + + public: pthread_mutex_t lock; pthread_mutex_t lock_ref_pos; pthread_mutex_t lock_est_pos_odo; @@ -136,33 +80,20 @@ struct robot { pthread_mutex_t lock_camera; /* competition parameters */ - enum team_color team_color; + team_colour team_color; /** State variable used for controlling the robot by pluggin * in and out start connector. */ - enum robot_start_state start_state; + robot_start_state start_state; /** Temporary storage for new trajectory. After the trajectory creation is * finished, this trajectory is submitted to fsmmove. */ - void *new_trajectory; - - unsigned char isTrajectory; - sem_t start; - - struct fsm_main_loop main_loop; - - /* partial FSMs */ - struct { - struct robo_fsm main; - struct robo_fsm motion; - struct robo_fsm act; - } fsm; /* actual position */ - struct robot_pos_type ref_pos; + robot_pos_type ref_pos; /* estimated position */ - struct robot_pos_type est_pos_odo; - struct robot_pos_type est_pos_indep_odo; + robot_pos_type est_pos_odo; + robot_pos_type est_pos_indep_odo; /** True if est_pos_odo is updated according to reception of motion_irc */ bool odometry_works; @@ -192,7 +123,7 @@ struct robot { struct odo_data_type odo_data; /* independent odometry */ struct corr_distances_type corr_distances; /* ultrasound */ - struct hokuyo_scan_type hokuyo; + hokuyo_scan_type hokuyo; bool ignore_hokuyo; /* variables for target detection */ @@ -208,32 +139,36 @@ struct robot { /** is set to true in separate thread when there is short time left */ bool short_time_to_end; bool check_turn_safety; + /** time when competition is started*/ + timespec startTime; + MoveHelper move_helper; + Scheduler sched; + /* State machines */ + Scheduler::processor_handle MAIN; + Scheduler::processor_handle MOTION; + + int init() __attribute__ ((warn_unused_result)); + int start() __attribute__ ((warn_unused_result)); + void robot_exit(); + void destroy(); + + void set_est_pos_trans(double x, double y, double phi){set_est_pos(trans_x(x), trans_y(y), trans_angle(phi));} + void set_est_pos(double x, double y, double phi); + void get_est_pos_trans(double &x, double &y, double &phi); + void get_est_pos(double &x, double &y, double &phi); }; /* robot */ -extern struct robot robot; +extern Robot robot; #ifdef __cplusplus extern "C" { #endif -int robot_init() __attribute__ ((warn_unused_result)); -int robot_start() __attribute__ ((warn_unused_result)); -void robot_exit(); -void robot_destroy(); - -void robot_get_est_pos_trans(double *x, double *y, double *phi); -void robot_get_est_pos(double *x, double *y, double *phi); /* Hack to easily disable display if it is not configured */ void serial_comm(int status); - -FSM_STATE_FULL_DECL(main, init); -FSM_STATE_FULL_DECL(motion, init); -FSM_STATE_FULL_DECL(disp, init); -FSM_STATE_FULL_DECL(act, wait_for_command); - #ifdef __cplusplus } #endif -- 2.39.2