motion-control.cc File Reference

#include <sys/time.h>
#include <time.h>
#include "trgen.h"
#include "balet.h"
#include "robodata.h"
#include <robot.h>
#include <pthread.h>
#include <path_planner.h>
#include <signal.h>
#include <movehelper.h>
#include <sharp.h>
#include <unistd.h>
#include <map.h>
#include "robot_config.h"
#include <robomath.h>
#include <ul_log.h>
#include "motion-control.h"
#include <ekf.h>
#include <geom.h>

Include dependency graph for motion-control.cc:

Defines
#define	DBG(format,...)
#define	DBGflush()
#define	MOTION_CONTROL
#define	SOUND_VELOCITY   (331.3+0.606*20)
#define	XCORR2METER   (SOUND_VELOCITY*(127.0/508.0)/3000.0)
#define	D_MAX   (XCORR2METER*508.0)
#define	MAX_POS_ERROR_M   0.25
	If the distance of robot's estimated position from robot's requested position if above this value, the robot lost and we try to reset localization.
#define	CLOSE_TO_TARGET_M   0.1
	If trajectory end is reached and robot's estimated position is closer than this distance, the movement is considered as "done".
#define	MOTION_PERIOD_NS   (50/*ms*/*1000*1000)
#define	MEASURE_TIMEOUT_NS   (100/*ms*/*1000*1000)
#define	SIG_DO_CONTROL_NOW   (SIGRTMIN+1)
Functions
	UL_LOG_CUST (ulogd_motion_control)
void *	thread_trajectory_follower (void *arg)
	A thread running the controller.
void	go (Trajectory *t, double append_time)
	Tells trajctory_follower to start moving along trajectory t.
void	stop ()
	switches to newly calculated trajectory to go on it at specific time
int	motion_control_init ()
	Initializes motion controller.
void	motion_control_done ()
void	get_future_pos (double rel_time_sec, Pos &pos, double &switch_time)
Variables
struct balet_params	k
sem_t	recalculation_not_running
sem_t	measurement_received
int	measurement_ok = 0
	Determines way of thread_trajectory_follower() operation: 0 measurement doesn't work, controller invocation based on time (formerly CONFIG_OPEN_LOOP) 2 measurement works, controller invocation based on sem_post 1 measurement doesn't work and stop() was called.

---

Detailed Description

Author:

Michal Sojka <sojkam1@fel.cvut.cz>, Petr Beneš

Date:

Fri Mar 20 10:36:59 2009

---

Define Documentation

#define CLOSE_TO_TARGET_M   0.1

If trajectory end is reached and robot's estimated position is closer than this distance, the movement is considered as "done".

#define D_MAX   (XCORR2METER*508.0)

#define DBG	(		format,
			...
	)

#define DBGflush

(

)

#define MAX_POS_ERROR_M   0.25

If the distance of robot's estimated position from robot's requested position if above this value, the robot lost and we try to reset localization.

#define MEASURE_TIMEOUT_NS   (100/*ms*/*1000*1000)

#define MOTION_CONTROL

#define MOTION_PERIOD_NS   (50/*ms*/*1000*1000)

#define SIG_DO_CONTROL_NOW   (SIGRTMIN+1)

#define SOUND_VELOCITY   (331.3+0.606*20)

#define XCORR2METER   (SOUND_VELOCITY*(127.0/508.0)/3000.0)

---

Function Documentation

void get_future_pos	(	double	rel_time_sec,
		Pos &	pos,
		double &	switch_time
	)

Here is the call graph for this function:

void go	(	Trajectory *	t,
		double	append_time
	)

Tells trajctory_follower to start moving along trajectory t.

Parameters:

	t	Trajectory to follow.
	append_time	Relative time from the beginning of the actual_trajectory when to append the new one

Here is the call graph for this function:

void motion_control_done

(

void

)

Here is the caller graph for this function:

int motion_control_init

(

void

)

Initializes motion controller.

Returns:

Zero on success, non-zero otherwise.

Here is the call graph for this function:

Here is the caller graph for this function:

void stop

(

void

)

switches to newly calculated trajectory to go on it at specific time

Here is the call graph for this function:

Here is the caller graph for this function:

void* thread_trajectory_follower

(

void *

arg

)

A thread running the controller.

This (high priority) thread executes the motion control algorithm. It calculates repference position based on actual trajectory and current time. Then it calls "balet" controller to close feedback.

Parameters:

arg

Returns:

Here is the call graph for this function:

Here is the caller graph for this function:

UL_LOG_CUST

(

ulogd_motion_control

)

---

Variable Documentation

struct balet_params k

Initial value:

 {
  p_tangent:  3,                
  p_angle: 2,                   
  p_perpen: 5                   



}

int measurement_ok = 0

Determines way of thread_trajectory_follower() operation:

• 0 measurement doesn't work, controller invocation based on time (formerly CONFIG_OPEN_LOOP)
• 2 measurement works, controller invocation based on sem_post
• 1 measurement doesn't work and stop() was called.

sem_t measurement_received

sem_t recalculation_not_running