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/motor_model.host.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'digital/mimot/src/dirty/motor_model.host.c') 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_; -- cgit v1.2.3