From 93589e0e32ff40c6f709e5a587bccfdcddd5b64e Mon Sep 17 00:00:00 2001 From: Petr Silhavik Date: Tue, 20 Nov 2012 19:30:22 +0100 Subject: [PATCH] Transform to C++: Scheduler in robot Prepare initialization of scheduler. Chenge pointers to references. --- src/boostFSM/robot.cc | 115 ++++++++++++++++++++---------------------- src/boostFSM/robot.h | 9 +++- 2 files changed, 63 insertions(+), 61 deletions(-) diff --git a/src/boostFSM/robot.cc b/src/boostFSM/robot.cc index 422c6154..7efd69d7 100644 --- a/src/boostFSM/robot.cc +++ b/src/boostFSM/robot.cc @@ -65,18 +65,12 @@ int Robot::init() 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) { + MAIN = sched.create_processor(); + MOTION = sched.create_processor(); + + team_color = VIOLET; + + if (team_color == VIOLET) { ul_loginf("We are VIOLET!\n"); } else { ul_loginf("We are RED!\n"); @@ -84,8 +78,8 @@ int Robot::init() 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); + ignore_hokuyo = false; + map = ShmapInit(1); //fill_in_known_areas_in_map(); signal(SIGINT, int_handler); @@ -93,14 +87,14 @@ int Robot::init() block_signals(); /* initial values */ - robot.orte.motion_speed.left = 0; - robot.orte.motion_speed.right = 0; + orte.motion_speed.left = 0; + orte.motion_speed.right = 0; - robot.orte.pwr_ctrl.voltage33 = 1; - robot.orte.pwr_ctrl.voltage50 = 1; - robot.orte.pwr_ctrl.voltage80 = 1; + orte.pwr_ctrl.voltage33 = 1; + orte.pwr_ctrl.voltage50 = 1; + orte.pwr_ctrl.voltage80 = 1; - robot.orte.camera_control.on = true; + orte.camera_control.on = true; //robot.fsm.motion.state = &fsm_state_motion_init; @@ -111,12 +105,12 @@ int Robot::init() 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; + obstacle_avoidance_enabled = true; + use_back_bumpers = true; + use_left_bumper = true; + use_right_bumper = true; + start_state = POWER_ON; + check_turn_safety = true; /* init ORTE domain, create publishers, subscribers, .. */ rv = robot_init_orte(); @@ -162,6 +156,8 @@ int Robot::start() perror("robot_start: pthread_create"); goto err; } + + sched(); // start main loop err: return rv; @@ -172,7 +168,7 @@ err: */ void Robot::robot_exit() { - // stop all fsm + sched.terminate(); } @@ -184,8 +180,9 @@ void Robot::destroy() { /*robot.motion.control_done();*/ - robottype_roboorte_destroy(&robot.orte); + robottype_roboortev_destroy(&orte); + sched.terminate(); // fsm_destroy(&robot.fsm.main); // fsm_destroy(&robot.fsm.motion); // fsm_destroy(&robot.fsm.act); @@ -194,31 +191,31 @@ void Robot::destroy() } -void Robot::get_est_pos_trans(double *x, double *y, double *phi) +void Robot::get_est_pos_trans(double &x, double &y, double &phi) { get_est_pos(x, y, phi); - *x = trans_x(*x); - *y = trans_y(*y); - *phi = trans_angle(*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) { - 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) { - 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; + if (indep_odometry_works) { + Guard g(&lock_est_pos_indep_odo); + x = est_pos_indep_odo.x; + y = est_pos_indep_odo.y; + phi = est_pos_indep_odo.phi; + } else if (odometry_works) { + Guard g(&lock_est_pos_odo); + x = est_pos_odo.x; + y = est_pos_odo.y; + phi = est_pos_odo.phi; } else { - Guard g(&robot.lock_ref_pos); - *x = robot.ref_pos.x; - *y = robot.ref_pos.y; - *phi = robot.ref_pos.phi; + Guard g(&lock_ref_pos); + x = ref_pos.x; + y = ref_pos.y; + phi = ref_pos.phi; } } @@ -233,23 +230,23 @@ void Robot::set_est_pos(double x, double y, double phi) if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M; { - Guard g(est_pos_indep_odo); - robot.est_pos_indep_odo.x = x; - robot.est_pos_indep_odo.y = y; - robot.est_pos_indep_odo.phi = phi; + Guard g(&est_pos_indep_odo); + est_pos_indep_odo.x = x; + est_pos_indep_odo.y = y; + est_pos_indep_odo.phi = phi; } { - Guard g(est_pos_odo); - robot.est_pos_odo.x = x; - robot.est_pos_odo.y = y; - robot.est_pos_odo.phi = phi; + Guard g(&est_pos_odo); + est_pos_odo.x = x; + est_pos_odo.y = y; + est_pos_odo.phi = phi; } { - Guard g(ref_pos); - robot.ref_pos.x = x; - robot.ref_pos.y = y; - robot.ref_pos.phi = phi; + Guard g(&ref_pos); + ref_pos.x = x; + ref_pos.y = y; + ref_pos.phi = phi; } } diff --git a/src/boostFSM/robot.h b/src/boostFSM/robot.h index 72be2292..e154bff2 100644 --- a/src/boostFSM/robot.h +++ b/src/boostFSM/robot.h @@ -13,6 +13,7 @@ #include #include #include "movehelper.h" +#include "scheduler.hpp" //#include //UL_LOG_CUST(ulogd_robot_h); /* Log domain name = ulogd + name of the file */ @@ -131,6 +132,10 @@ class Robot float odo_distance_a; float odo_distance_b; Actuators act; + Scheduler sched; + /* State machines */ + Scheduler::processor_handle MAIN; + Scheduler::processor_handle MOTION; /* Class methods */ Robot(); ~Robot(); @@ -140,8 +145,8 @@ class Robot 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); + void get_est_pos_trans(double &x, double &y, double &phi); + void get_est_pos(double &x, double &y, double &phi); }; extern Robot robot; -- 2.39.2