From: Michal Sojka Date: Fri, 11 Feb 2011 16:11:08 +0000 (+0100) Subject: I got rid of ultrasound localization and corns-config in demo. X-Git-Url: https://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/213867b48d4d640c81767f4fa85efd9de00458e1 I got rid of ultrasound localization and corns-config in demo. --- diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk index d4302df9..924a30d0 100644 --- a/src/robofsm/Makefile.omk +++ b/src/robofsm/Makefile.omk @@ -7,19 +7,14 @@ default_CONFIG = CONFIG_LOCK_CHECKING=n HAVE_PRIO_INHERIT=y config_include_HEADERS = robot_config.h robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT -bin_PROGRAMS += competition -competition_SOURCES = competition.cc common-states.cc \ - strategy_opp_corn.cc strategy_opp_oranges.cc \ - strategy_12_oranges.cc strategy_six_oranges.cc - -bin_PROGRAMS += homologation -homologation_SOURCES = homologation.cc +#bin_PROGRAMS += demo +#demo_SOURCES = demo.cc # Library with general support functions for the robot lib_LIBRARIES += robot robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc \ motion-control.cc map_handling.c \ - match-timing.c eb2010misc.cc + match-timing.c robot_GEN_SOURCES = roboevent.c include_GEN_HEADERS += roboevent.h @@ -28,13 +23,10 @@ include_HEADERS += robot.h movehelper.h robot_orte.h actuators.h lib_LIBRARIES += actlib actlib_SOURCES = actuators.c -lib_LIBRARIES += cornslib -cornslib_SOURCES = corns_configs.c - # Libraries linked to all programs in this Makefile lib_LOADLIBES = robot mcl robomath roboorte robottype pthread rt m \ orte pathplan sharp map fsm rbtree motion robodim \ - actlib cornslib ulut + actlib ulut # Automatic generation of event definition files include-pass_HOOKS = roboevent.c roboevent.h diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc index 7f2e99de..f3a66130 100644 --- a/src/robofsm/fsmmove.cc +++ b/src/robofsm/fsmmove.cc @@ -318,14 +318,6 @@ FSM_STATE(movement) { switch (FSM_EVENT) { case EV_ENTRY: - if (robot.localization_works) { - robot_pos_type p; - ROBOT_LOCK(est_pos_uzv); - p = robot.est_pos_uzv; - ROBOT_UNLOCK(est_pos_uzv); - - robot_set_est_pos_notrans(p.x, p.y, p.phi); - } break; case EV_TRAJECTORY_DONE: // Skip close to target because sometimes it turn the robot to much diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c index 87028b1f..077703de 100644 --- a/src/robofsm/robot.c +++ b/src/robofsm/robot.c @@ -1,5 +1,5 @@ /* - * robot_eb2008.c 08/04/20 + * robot_demo2011.c * * Robot's generic initialization and clean up functions. * @@ -23,7 +23,6 @@ #include "map_handling.h" #include #include "actuators.h" -#include "corns_configs.h" #include #include @@ -79,14 +78,6 @@ void fill_in_known_areas_in_map() ShmapSetRectangleFlag(0.6, 1.45, 2.45, 1.55, MAP_FLAG_WALL, 0); /* plateau, slopes */ ShmapSetRectangleFlag(1.46, PLAYGROUND_HEIGHT_M - 0.5, 1.57, PLAYGROUND_HEIGHT_M, MAP_FLAG_WALL, 0); /* plateau, slopes */ - /* corns */ - struct corns_group *corns = get_all_corns(0, 0); - struct corn * corn; - for(corn = corns->corns; corn < &corns->corns[corns->corns_count]; corn++) { - ShmapSetCircleFlag(corn->position.x, corn->position.y, CORN_NEIGHBOURHOOD_RADIUS_M, MAP_FLAG_WALL, 0); - } - dispose_corns_group(corns); // robot.corns will be set later, when the corns' configuration is known - ShmapSetRectangleFlag(0.32, 0.25, 0.38, 0.55, 0, MAP_FLAG_WALL); /* destination position near blue container */ ShmapSetRectangleFlag(2.62, 0.25, 2.68, 0.55, 0, MAP_FLAG_WALL); /* destination position near yellow container */ } @@ -103,13 +94,13 @@ 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); } - + } -/** +/** * Initializes the robot. * Setup fields in robot structure, initializes FSMs and ORTE. - * + * * @return 0 */ int robot_init() @@ -123,7 +114,6 @@ int robot_init() #endif pthread_mutex_init(&robot.lock, &mattr); pthread_mutex_init(&robot.lock_ref_pos, &mattr); - pthread_mutex_init(&robot.lock_est_pos_uzv, &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); @@ -131,12 +121,8 @@ int robot_init() 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, "motion", &robot.main_loop); - fsm_init(&robot.fsm.display, "display", &robot.main_loop); - fsm_init(&robot.fsm.act, "actuators", &robot.main_loop); */ 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); @@ -152,8 +138,8 @@ int robot_init() ul_loginf("We are BLUE!\n"); } - robot_set_est_pos_trans(0.16, PLAYGROUND_HEIGHT_M - 0.16, DEG2RAD(-45)); - + robot_set_est_pos_trans(1, 1, DEG2RAD(0)); + robot.ignore_hokuyo = false; robot.map = ShmapInit(1); fill_in_known_areas_in_map(); @@ -171,7 +157,7 @@ int robot_init() 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 */ @@ -195,7 +181,7 @@ int robot_init() return rv; } -/** +/** * Starts the robot FSMs and threads. * * @return 0 @@ -227,7 +213,7 @@ int robot_start() perror("robot_start: pthread_attr_setschedparam()"); goto err; } - rv = pthread_create(&thr_obstacle_forgeting, + rv = pthread_create(&thr_obstacle_forgeting, &tattr, thread_obstacle_forgeting, NULL); if (rv) { perror("robot_start: pthread_create"); @@ -242,7 +228,7 @@ err: return rv; } -/** +/** * Signals all the robot threads to finish. */ void robot_exit() @@ -253,7 +239,7 @@ void robot_exit() fsm_exit(&robot.fsm.act); } -/** +/** * Stops the robot. All resources alocated by robot_init() or * robot_start() are dealocated here. */ @@ -262,7 +248,7 @@ void robot_destroy() motion_control_done(); // FIXME: set actuators to well defined position (FJ) - + robottype_roboorte_destroy(&robot.orte); fsm_destroy(&robot.fsm.main); diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h index 88ae8b16..84a24b59 100644 --- a/src/robofsm/robot.h +++ b/src/robofsm/robot.h @@ -52,7 +52,7 @@ enum robot_start_state { #define FSM_GET_BY_ID(fsm_id) (&robot.fsm.ROBOT_FSM_##fsm_id) /** - * LOCKING MANIPULATION + * LOCKING MANIPULATION */ #ifdef CONFIG_LOCK_CHECKING @@ -73,7 +73,7 @@ extern struct lock_log robot_lock_log; #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. */ @@ -83,7 +83,7 @@ extern struct lock_log robot_lock_log; __LOCK_CHECK(&robot.__robot_lock_##var); \ } while(0) -/** +/** * Unlocks the robot structure. * @param var Field in the structure, which was locked by ROBOT_LOCK. */ @@ -96,7 +96,6 @@ extern struct lock_log robot_lock_log; /* 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_uzv lock_est_pos_uzv #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 @@ -106,7 +105,6 @@ extern struct lock_log robot_lock_log; #define __robot_lock_hokuyo lock #define __robot_lock_cmu lock #define __robot_lock_bumper lock -#define __robot_lock_drives lock #define __robot_lock_disp lock_disp #define __robot_lock_motion_irc lock #define __robot_lock_odo_data lock @@ -127,7 +125,7 @@ enum robot_component { COMPONENT_HOKUYO, COMPONENT_START, COMPONENT_VIDLE, - + ROBOT_COMPONENT_NUMBER }; @@ -135,7 +133,6 @@ enum robot_component { struct robot { pthread_mutex_t lock; pthread_mutex_t lock_ref_pos; - pthread_mutex_t lock_est_pos_uzv; pthread_mutex_t lock_est_pos_odo; pthread_mutex_t lock_est_pos_indep_odo; pthread_mutex_t lock_meas_angles; @@ -153,7 +150,7 @@ struct robot { /** 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; @@ -169,18 +166,14 @@ struct robot { /* actual position */ struct robot_pos_type ref_pos; /* estimated position */ - struct robot_pos_type est_pos_uzv; struct robot_pos_type est_pos_odo; struct robot_pos_type est_pos_indep_odo; - /** True if localization data arrives correctly and therfore - * localization runs */ - bool localization_works; /** True if est_pos_odo is updated according to reception of motion_irc */ bool odometry_works; /** True if est_pos_indep_odo is updated according to reception of motion_indep_odo */ bool indep_odometry_works; - + bool use_back_switch; bool use_left_switch; bool use_right_switch; @@ -195,7 +188,7 @@ struct robot { * (taken as sudden zero velocities) */ bool motion_irc_received; - + /* orte */ struct robottype_orte_data orte; @@ -226,7 +219,7 @@ extern struct robot robot; #ifdef __cplusplus extern "C" { -#endif +#endif int robot_init() __attribute__ ((warn_unused_result)); int robot_start() __attribute__ ((warn_unused_result)); @@ -240,7 +233,7 @@ void robot_get_est_pos(double *x, double *y, double *phi); void serial_comm(int status); - + FSM_STATE_FULL_DECL(main, init); FSM_STATE_FULL_DECL(motion, init); FSM_STATE_FULL_DECL(disp, init); @@ -248,7 +241,7 @@ FSM_STATE_FULL_DECL(act, wait_for_command); #ifdef __cplusplus } -#endif +#endif /*Thread priorities*/ #define THREAD_PRIO_TRAJ_FOLLOWER 90 /* As high as possible */ diff --git a/src/robofsm/test/lineavoid.cc b/src/robofsm/test/lineavoid.cc index 82141564..af05b082 100644 --- a/src/robofsm/test/lineavoid.cc +++ b/src/robofsm/test/lineavoid.cc @@ -37,11 +37,9 @@ FSM_STATE(init) { break; case EV_TIMER: FSM_TRANSITION(robot_goto_test); - if (!robot.localization_works) { - robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2, - PLAYGROUND_HEIGHT_M/2, - DEG2RAD(0)); - } + robot_set_est_pos_trans(PLAYGROUND_WIDTH_M/2, + PLAYGROUND_HEIGHT_M/2, + DEG2RAD(0)); break; default:; } diff --git a/src/robofsm/test/rectangle.cc b/src/robofsm/test/rectangle.cc index 803abdcd..53e55a5c 100644 --- a/src/robofsm/test/rectangle.cc +++ b/src/robofsm/test/rectangle.cc @@ -31,12 +31,7 @@ FSM_STATE(init) { break; case EV_TIMER: FSM_TRANSITION(rectangle); - if (!robot.localization_works) { - robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0)); - } else { - // Set odo position - robot_set_est_pos_trans(robot.est_pos_uzv.x, robot.est_pos_uzv.y, DEG2RAD(0)); - } + robot_set_est_pos_trans(0.5, 0.5, DEG2RAD(0)); break; default:; } @@ -95,7 +90,7 @@ FSM_STATE(turn) switch (FSM_EVENT) { case EV_ENTRY: robot_trajectory_new(NULL); - robot_trajectory_add_final_point_trans(1, 0.5, + robot_trajectory_add_final_point_trans(1, 0.5, TURN_CCW(DEG2RAD(0))); break; case EV_MOTION_DONE: