From 50b8abe7f025cd2a4a498c24da37826a6e2b585a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 4 May 2010 02:24:57 +0200 Subject: digital/mimot/src/dirty: add motor hardware limits See r1069. --- digital/mimot/src/dirty/models.host.c | 3 +++ digital/mimot/src/dirty/motor_model.host.c | 12 ++++++++++++ digital/mimot/src/dirty/motor_model.host.h | 3 +++ 3 files changed, 18 insertions(+) (limited to 'digital/mimot') diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index ab7242db..fe1761c3 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/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" @@ -47,6 +48,8 @@ static const struct motor_def_t marcel_clamp_f2342_model = /* Load characteristics. */ 0.100 * 0.005 * 0.005,/* Load (kg.m^2). */ /* This is a pifometric estimation. */ + /* Hardware limits. */ + 0.0, +INFINITY, }; /* Marcel, APBTeam 2010. */ diff --git a/digital/mimot/src/dirty/motor_model.host.c b/digital/mimot/src/dirty/motor_model.host.c index 37171c32..f87e7db6 100644 --- a/digital/mimot/src/dirty/motor_model.host.c +++ b/digital/mimot/src/dirty/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/mimot/src/dirty/motor_model.host.h b/digital/mimot/src/dirty/motor_model.host.h index 3a04e15a..283b6fd6 100644 --- a/digital/mimot/src/dirty/motor_model.host.h +++ b/digital/mimot/src/dirty/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