From da016975f5bcc59d9e223fcc1ddbc50bb7ebaa03 Mon Sep 17 00:00:00 2001 From: Petr Silhavik Date: Tue, 25 Dec 2012 20:04:36 +0100 Subject: [PATCH] robofsm: Motion_control Move global variables to struct. Make constants from defines. --- src/robofsm/motion-control.cc | 80 ++++++++++++++++++----------------- 1 file changed, 42 insertions(+), 38 deletions(-) diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc index 96efa0a4..7e022b75 100644 --- a/src/robofsm/motion-control.cc +++ b/src/robofsm/motion-control.cc @@ -60,13 +60,13 @@ UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file * requested position if above this value, the robot lost and we try * to reset localization. */ -#define MAX_POS_ERROR_M 0.25 +const float MAX_POS_ERROR_M = 0.25; /** * If trajectory end is reached and robot's estimated position is * closer than this distance, the movement is considered as "done". */ -#define CLOSE_TO_TARGET_M 0.1 +const float CLOSE_TO_TARGET_M = 0.1; //Controller gains const struct balet_params k = { @@ -84,12 +84,16 @@ const struct balet_params k = { #define SIG_DO_CONTROL_NOW (SIGRTMIN+1) // Global varibles -static pthread_t thr_trajectory_follower; -static struct timeval tv_start; /**< Absolute time, when trajectory started. */ +struct MotionControlHandler { +pthread_t thr_trajectory_follower; +struct timeval tv_start; /**< Absolute time, when trajectory started. */ /** Stores the actually followed trajectory object */ -static Trajectory *actual_trajectory; -static pthread_mutex_t actual_trajectory_lock; +Trajectory *actual_trajectory; +pthread_mutex_t actual_trajectory_lock; +}; + +MotionControlHandler motion_handler; // Trajectory recalculation sem_t recalculation_not_running; @@ -109,12 +113,12 @@ static void delete_actual_trajectory() { Trajectory *old; { - Guard g(actual_trajectory_lock); - old = actual_trajectory; - actual_trajectory = NULL; + Guard g(motion_handler.actual_trajectory_lock); + old = motion_handler.actual_trajectory; + motion_handler.actual_trajectory = NULL; } robot.move_helper.send_speed(0,0); - if (old) delete(old); + if (old) delete old; } /** Sends events from follower thread to FSM. */ @@ -184,8 +188,8 @@ static void do_control() // Calculate reference position /***FIXME:should not rely on system clock, the period is fixed***/ gettimeofday(&tv, NULL); - t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0; - t += (tv.tv_sec - tv_start.tv_sec); + t = (double)(tv.tv_usec - motion_handler.tv_start.tv_usec) / 1000000.0; + t += (tv.tv_sec - motion_handler.tv_start.tv_sec); /* // check for new trajectory to switch // only if the trajectory is already prepared @@ -200,16 +204,16 @@ static void do_control() pthread_mutex_unlock(&switch_to_trajectory_lock); } */ - Guard g(actual_trajectory_lock); - Trajectory *w = actual_trajectory; + Guard g(motion_handler.actual_trajectory_lock); + Trajectory *w = motion_handler.actual_trajectory; if (w) { Pos ref_pos, est_pos, balet_out; bool done; // Calculate reference position gettimeofday(&tv, NULL); - t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0; - t += (tv.tv_sec - tv_start.tv_sec); + t = (double)(tv.tv_usec - motion_handler.tv_start.tv_usec) / 1000000.0; + t += (tv.tv_sec - motion_handler.tv_start.tv_sec); // if switch_to_trajectory is being prepared, it can not stop calculation // and start to count again, it could evoke overloading @@ -219,8 +223,8 @@ static void do_control() done = w->getRefPos(t, ref_pos); - if (ref_pos.omega > actual_trajectory->constr.maxomega) - DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega); + if (ref_pos.omega > motion_handler.actual_trajectory->constr.maxomega) + DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, motion_handler.actual_trajectory->constr.maxomega); { Guard g(robot.lock_ref_pos); robot.ref_pos.x = ref_pos.x; @@ -280,7 +284,7 @@ void *thread_trajectory_follower(void *arg) int ret; struct sched_param param; - param.sched_priority = THREAD_PRIO_TRAJ_FOLLOWER; + param.sched_priority = 99; if (pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m) != 0) perror("Warning: Cannot set RT priority for thread_prio_traj_follower()"); @@ -317,19 +321,19 @@ void *thread_trajectory_follower(void *arg) */ void go(Trajectory *t, double append_time) { - Guard g(actual_trajectory_lock); + Guard g(motion_handler.actual_trajectory_lock); Trajectory *old; - if (actual_trajectory && append_time != 0) { + if (motion_handler.actual_trajectory && append_time != 0) { // trajectory only connects a new one in some specific time - if(!actual_trajectory->appendTrajectory(*t, append_time)) + if(!motion_handler.actual_trajectory->appendTrajectory(*t, append_time)) DBG("Can not append trajectory\n"); } else { // trajectory starts from zero time - old = actual_trajectory; - gettimeofday(&tv_start, NULL); - actual_trajectory = t; + old = motion_handler.actual_trajectory; + gettimeofday(&(motion_handler.tv_start), NULL); + motion_handler.actual_trajectory = t; #ifdef MOTION_LOG - t->logTraj(tv_start.tv_sec + 1e-6*tv_start.tv_usec); + t->logTraj(motion_handler.tv_start.tv_sec + 1e-6*motion_handler.tv_start.tv_usec); #endif if (old) delete(old); @@ -374,7 +378,7 @@ int motion_control_init() pthread_mutexattr_t mattr; int ret; - actual_trajectory = NULL; + motion_handler.actual_trajectory = NULL; //switch_to_trajectory = NULL; @@ -382,13 +386,13 @@ int motion_control_init() #ifdef HAVE_PRIO_INHERIT ret = pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT); #endif - pthread_mutex_init(&actual_trajectory_lock, &mattr); + pthread_mutex_init(&(motion_handler.actual_trajectory_lock), &mattr); sem_init(&recalculation_not_running, 0, 1); // Trajectory follower thread sem_init(&measurement_received, 0, 0); - ret = pthread_create(&thr_trajectory_follower, NULL, thread_trajectory_follower, NULL); + ret = pthread_create(&(motion_handler.thr_trajectory_follower), NULL, thread_trajectory_follower, NULL); if(ret) { perror("move_init: pthread_create"); goto err; @@ -401,8 +405,8 @@ int motion_control_init() void motion_control_done() { - pthread_cancel(thr_trajectory_follower); - pthread_join(thr_trajectory_follower, NULL); + pthread_cancel(motion_handler.thr_trajectory_follower); + pthread_join(motion_handler.thr_trajectory_follower, NULL); robot.orte.motion_speed.right = 0; robot.orte.motion_speed.left = 0; @@ -415,17 +419,17 @@ void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time) struct timeval tv; gettimeofday(&tv, NULL); - switch_time = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0; - switch_time += (tv.tv_sec - tv_start.tv_sec); + switch_time = (double)(tv.tv_usec - motion_handler.tv_start.tv_usec) / 1000000.0; + switch_time += (tv.tv_sec - motion_handler.tv_start.tv_sec); switch_time += rel_time_sec; - pthread_mutex_lock(&actual_trajectory_lock); - if (actual_trajectory) { - actual_trajectory->getRefPos(switch_time, pos); - pthread_mutex_unlock(&actual_trajectory_lock); + pthread_mutex_lock(&(motion_handler.actual_trajectory_lock)); + if (motion_handler.actual_trajectory) { + motion_handler.actual_trajectory->getRefPos(switch_time, pos); + pthread_mutex_unlock(&(motion_handler.actual_trajectory_lock)); } else { // Robot doesn't move, so return current position - pthread_mutex_unlock(&actual_trajectory_lock); + pthread_mutex_unlock(&(motion_handler.actual_trajectory_lock)); robot.get_est_pos(pos.x, pos.y, pos.phi); pos.v = 0; -- 2.39.2