LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

avec ou sans diagonale

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-06-09 17:00:45 +02:00
parent 1374badd9e
commit 7032e7aa49
2 changed files with 18 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
cfg/Commander.cfg
View file

@ -14,4 +14,5 @@ gen.add("angle_vel", double_t, 0, "Angular velocity", 0.01, 0., 10.)
gen.add("x_vel", double_t, 0, "X Translation velocity", 0.5, 0., 10.) gen.add("x_vel", double_t, 0, "X Translation velocity", 0.5, 0., 10.)
gen.add("y_vel", double_t, 0, "Y Translation velocity", 0.3, 0., 10.) gen.add("y_vel", double_t, 0, "Y Translation velocity", 0.3, 0., 10.)
gen.add("z_vel", double_t, 0, "Vertical translation velocity", 1.5, 0., 10.) gen.add("z_vel", double_t, 0, "Vertical translation velocity", 1.5, 0., 10.)
gen.add("no_diag", bool_t, 0, "Drone cannot translate on a diagonal direction", True)
exit(gen.generate(PACKAGE, "hand_control", "Commander")) exit(gen.generate(PACKAGE, "hand_control", "Commander"))

17
src/commander.cpp
View file

@ -35,7 +35,7 @@ class Run
float z_dev_min, x_dev_min, y_dev_min, th_dev_min; float z_dev_min, x_dev_min, y_dev_min, th_dev_min;
uint64_t min_number; uint64_t min_number;
bool first_msg; bool no_diag;
ros::Publisher pub; ros::Publisher pub;
@ -51,6 +51,8 @@ class Run
mvt->linear.z = zz * z_vel; mvt->linear.z = zz * z_vel;
} }
if (no_diag)
{
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min) if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
{ {
mvt->linear.y = yy * y_vel; mvt->linear.y = yy * y_vel;
@ -59,6 +61,13 @@ class Run
{ {
mvt->linear.x = - xx * x_vel; mvt->linear.x = - xx * x_vel;
} }
} else
{
if (fabs(yy) > y_dev_min)
mvt->linear.y = yy * y_vel;
if (fabs(xx) > x_dev_min)
mvt->linear.x = - xx * x_vel;
}
if (fabs(theta) > th_dev_min) { if (fabs(theta) > th_dev_min) {
mvt->angular.z = theta * angle_vel; mvt->angular.z = theta * angle_vel;
@ -97,11 +106,6 @@ class Run
theta = msg->angle; theta = msg->angle;
// theta between -90 and 90 // theta between -90 and 90
if (first_msg)
{
first_msg = false;
ROS_INFO("first msg received");
}
ROS_INFO("coords updated"); ROS_INFO("coords updated");
} else { } else {
xx = yy = zz = 0.; xx = yy = zz = 0.;
@ -122,6 +126,7 @@ class Run
y_vel = c.y_vel; y_vel = c.y_vel;
z_vel = c.z_vel; z_vel = c.z_vel;
angle_vel = c.angle_vel; angle_vel = c.angle_vel;
no_diag = c.no_diag;
} }
void run() void run()