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

finish commmander_atan with linear function

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-30 14:05:04 +02:00
parent 571c2ed11c
commit 8345801ef7
2 changed files with 42 additions and 29 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
cfg/Commander_atan.cfg
View file

@ -17,4 +17,5 @@ gen.add("x_min", double_t, 0, "min x coord to be published", 0.1, 0., 1.)
gen.add("y_min", double_t, 0, "min y coord to be published", 0.1, 0., 1.)
gen.add("z_min", double_t, 0, "min z coord to be published", 0.1, 0., 1.)
gen.add("th_min", double_t, 0, "min th coord to be published", 0.1, 0., 1.)
gen.add("atan", bool_t, 0, "use atan function instead of linear function", False)
exit(gen.generate(PACKAGE, "hand_control", "Commander_atan"))

70
src/commander_atan.cpp
View file

@ -24,23 +24,9 @@ class Run
{
protected:
float max;
float gradient;
public:
virtual float f(const float& arg) = 0;
Function() : max(0.8) {}
Function(const float& maximum, const float& grad)
{
set_gradient(grad);
set_max(maximum);
}
virtual void set_gradient(const float& grad)
{
gradient = grad;
}
void set_max(const float& m)
{
max = m;
}
virtual void set(const float& maximum, const float& grad) = 0;
};
class Atan : public Function
@ -56,26 +42,38 @@ class Run
Atan(const float& max, const float& grad)
:t(demi_pi)
{
set_gradient(grad);
set(max, grad);
}
virtual void set_gradient(const float& grad)
virtual void set(const float& maximum, const float& grad)
{
t = grad*2*atan(1);
gradient = grad;
max = maximum;
t = grad*demi_pi/max;
}
};
class Lin : public Function
{
private:
float t;
public:
Lin(const float& max, const float& grad){}
Lin(const float& max, const float& grad)
{
set(max, grad);
}
virtual void set(const float& maximum, const float& grad)
{
max = maximum;
t = grad;
}
virtual float f(const float& arg)
{
return std::max(gradient * arg, max);
return std::max(t * arg, max);
}
};
enum FType { f_lin, f_atan };
Data<boost::shared_ptr<Function> > f;
Data<FType> f_type;
Data<float> min;
float neutral_z;
float max_curv;
@ -89,6 +87,7 @@ class Run
f.y = boost::make_shared<Lin>(0.8f, 0.5f);
f.z = boost::make_shared<Lin>(0.8f, 2.0f);
f.th = boost::make_shared<Lin>(0.8f, 0.005f);
f_type.x = f_type.y = f_type.z = f_type.th = f_lin;
min.x = min.y = min.z = min.th = 0.1;
}
@ -142,14 +141,27 @@ class Run
max_curv = c.max_curvature;
neutral_z = c.neutral_alt;
min_number = c.min_points_number;
f.x->set_gradient(c.p_x);
f.y->set_gradient(c.p_y);
f.z->set_gradient(c.p_z);
f.th->set_gradient(c.p_th);
f.x->set_max(c.max_x);
f.y->set_max(c.max_y);
f.z->set_max(c.max_z);
f.th->set_max(c.max_th);
if (c.atan and f_type.x != f_atan)
{
f.x = boost::make_shared<Atan>(0.8f, 0.5f);
f.y = boost::make_shared<Atan>(0.8f, 0.5f);
f.z = boost::make_shared<Atan>(0.8f, 2.f);
f.th = boost::make_shared<Atan>(0.8f, 0.005f);
f_type.x = f_type.y = f_type.z = f_type.th = f_atan;
} else if (!c.atan and f_type.x == f_atan) {
f.x = boost::make_shared<Lin>(0.8f, 0.5f);
f.y = boost::make_shared<Lin>(0.8f, 0.5f);
f.z = boost::make_shared<Lin>(0.8f, 2.0f);
f.th = boost::make_shared<Lin>(0.8f, 0.005f);
f_type.x = f_type.y = f_type.z = f_type.th = f_lin;
}
f.x->set(c.max_x, c.p_x);
f.y->set(c.max_y, c.p_y);
f.z->set(c.max_z, c.p_z);
f.th->set(c.max_th, c.p_th);
min.x = c.x_min;
min.y = c.y_min;
min.z = c.z_min;