summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/models.host.c15
-rw-r--r--digital/asserv/src/asserv/motor_model.host.c12
-rw-r--r--digital/asserv/src/asserv/motor_model.host.h3
3 files changed, 30 insertions, 0 deletions
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index 31209df2..cba0d458 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -22,6 +22,7 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#define _GNU_SOURCE 1 /* Need ISO C99 features as well. */
#include "common.h"
#include "motor_model.host.h"
@@ -46,6 +47,8 @@ static const struct motor_def_t re25cll_model =
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
0.0, /* Load (kg.m^2). */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
};
/* RE25G with 1:20.25 gearbox model. */
@@ -63,6 +66,8 @@ static const struct motor_def_t re25g_model =
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
0.0, /* Load (kg.m^2). */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
};
/* AMAX32GHP with 1:16 gearbox model. */
@@ -80,6 +85,8 @@ static const struct motor_def_t amax32ghp_model =
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
0.0, /* Load (kg.m^2). */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
};
/* Gloubi, Efrei 2006. */
@@ -159,6 +166,8 @@ static const struct motor_def_t giboulee_arm_model =
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
0.200 * 0.1 * 0.1, /* Load (kg.m^2). */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
};
/* Giboulée, APBTeam 2008. */
@@ -205,6 +214,8 @@ static const struct motor_def_t aquajim_arm_model =
0.75, /* Gearbox efficiency. */
/* Load characteristics. */
0.05 * 2.5 * 0.06 * 0.06, /* Load (kg.m^2). */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
};
/* AquaJim elevator model, with a RE25CLL and a 1:10 ratio gearbox. */
@@ -225,6 +236,8 @@ static const struct motor_def_t aquajim_elevator_model =
/* Load characteristics. */
0.200 * 0.01 * 0.01,/* Load (kg.m^2). */
/* This is a pifometric estimation. */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
};
/* AquaJim, APBTeam 2009. */
@@ -272,6 +285,8 @@ static const struct motor_def_t marcel_elevator_model =
/* Load characteristics. */
1.0 * 0.01 * 0.01, /* Load (kg.m^2). */
/* This is a pifometric estimation. */
+ /* Hardware limits. */
+ 0.0, +INFINITY,
};
/* Marcel, APBTeam 2010. */
diff --git a/digital/asserv/src/asserv/motor_model.host.c b/digital/asserv/src/asserv/motor_model.host.c
index 37171c32..f87e7db6 100644
--- a/digital/asserv/src/asserv/motor_model.host.c
+++ b/digital/asserv/src/asserv/motor_model.host.c
@@ -77,6 +77,18 @@ void motor_model_step (struct motor_t *m)
+ h * m->m.i_G * m->m.i_G * m->m.ro_G / m->m.J
* (m->m.Kt * m->i - m->m.Rf * m->o);
th_ = m->th + h * m->o;
+ /* Test for limits overflow, I have no proof it works right for the
+ * moment, only suspicions. */
+ if (th_ < m->m.th_min)
+ {
+ th_ = m->m.th_min;
+ o_ = 0.0;
+ }
+ else if (th_ > m->m.th_max)
+ {
+ th_ = m->m.th_max;
+ o_ = 0.0;
+ }
/* Ok, now store this step. */
m->i = i_;
m->o = o_;
diff --git a/digital/asserv/src/asserv/motor_model.host.h b/digital/asserv/src/asserv/motor_model.host.h
index 3a04e15a..283b6fd6 100644
--- a/digital/asserv/src/asserv/motor_model.host.h
+++ b/digital/asserv/src/asserv/motor_model.host.h
@@ -40,6 +40,9 @@ struct motor_def_t
double ro_G;/* Gearbox efficiency. */
/* Load characteristics. */
double J; /* Load at gearbox output (kg.m^2). */
+ /* Hardware limits (use +/-INFINITY for none). */
+ double th_min; /* Minimum theta value. */
+ double th_max; /* Maximum theta value. */
};
/** Motor and load characteristics and current data. Angular speed and theta