From 756ced5e37dcb240bcbfa4faf94153b38327d026 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 2 May 2010 10:08:35 +0200 Subject: digital/asserv/src/asserv: add motor hardware limits --- digital/asserv/src/asserv/models.host.c | 15 +++++++++++++++ digital/asserv/src/asserv/motor_model.host.c | 12 ++++++++++++ digital/asserv/src/asserv/motor_model.host.h | 3 +++ 3 files changed, 30 insertions(+) (limited to 'digital') 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 -- cgit v1.2.3