From 5948ad3b390c3c7a8deac3fea4aa6f6e93f395f7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 18 Dec 2011 15:16:09 +0100 Subject: digital/avr/modules/motor: add motor model --- .../avr/modules/motor/motor_model/Makefile.module | 1 + .../modules/motor/motor_model/motor_model.host.c | 92 +++++++++++++ .../modules/motor/motor_model/motor_model.host.h | 69 ++++++++++ .../motor/motor_model/motor_model_defs.host.c | 148 +++++++++++++++++++++ .../motor/motor_model/motor_model_defs.host.h | 40 ++++++ .../avr/modules/motor/motor_model/test/Makefile | 10 ++ .../motor/motor_model/test/test_motor_model.c | 79 +++++++++++ 7 files changed, 439 insertions(+) create mode 100644 digital/avr/modules/motor/motor_model/Makefile.module create mode 100644 digital/avr/modules/motor/motor_model/motor_model.host.c create mode 100644 digital/avr/modules/motor/motor_model/motor_model.host.h create mode 100644 digital/avr/modules/motor/motor_model/motor_model_defs.host.c create mode 100644 digital/avr/modules/motor/motor_model/motor_model_defs.host.h create mode 100644 digital/avr/modules/motor/motor_model/test/Makefile create mode 100644 digital/avr/modules/motor/motor_model/test/test_motor_model.c (limited to 'digital') diff --git a/digital/avr/modules/motor/motor_model/Makefile.module b/digital/avr/modules/motor/motor_model/Makefile.module new file mode 100644 index 00000000..c659f5ef --- /dev/null +++ b/digital/avr/modules/motor/motor_model/Makefile.module @@ -0,0 +1 @@ +motor_motor_model_SOURCES = motor_model.host.c motor_model_defs.host.c diff --git a/digital/avr/modules/motor/motor_model/motor_model.host.c b/digital/avr/modules/motor/motor_model/motor_model.host.c new file mode 100644 index 00000000..98112d66 --- /dev/null +++ b/digital/avr/modules/motor/motor_model/motor_model.host.c @@ -0,0 +1,92 @@ +/* motor_model.c - DC motor model. */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "motor_model.host.h" + +/** + * This file models a DC motor. There is two parts, electric model and + * mechanic model. + * + * You can find documentation about this subject on: + * - maxon motor web site: http://www.maxonmotor.com/ + * + * From there, you can find the following differential equations: + * u(t) = R i(t) + L di(t)/dt + 1/Ke o(t) + * J do(t)/dt = Kt i(t) - Rf o(t) + * dth(t)/dt = o(t) + * + * (see motor_t for variables description) + * + * The gearbox reduction and efficiency have to be added in the equations, + * this is left as an exercice for the reader. + * + * Those equations can be solved numerically thanks to Euler method: + * + * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t)) + * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t)) + * th(t+h) = th(t) + h o(t) + */ + +/** Make a simulation step. */ +void motor_model_step (motor_model_t *m) +{ + double i_, o_, th_; /* New computed values. */ + double h; /* Infinitesimal step... Well, not so infinite. */ + int d; + d = m->d; + h = m->h / d; + /* Make several small steps to increase precision. */ + for (; d; d--) + { + /* Make one little step. */ + i_ = m->i + + h * (1.0 / m->m.L * m->u + - m->m.R / m->m.L * m->i + - 1.0 / m->m.Ke / m->m.L * m->o); + o_ = m->o + + 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_; + m->th = th_; + } + /* Finished! */ + m->t += m->h; +} + diff --git a/digital/avr/modules/motor/motor_model/motor_model.host.h b/digital/avr/modules/motor/motor_model/motor_model.host.h new file mode 100644 index 00000000..deb55b92 --- /dev/null +++ b/digital/avr/modules/motor/motor_model/motor_model.host.h @@ -0,0 +1,69 @@ +#ifndef motor_model_host_h +#define motor_model_host_h +/* motor_model.host.h - DC motor model. */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Motor and load characteristics. */ +struct motor_model_def_t +{ + /* Motor characteristics. */ + double Ke; /* Speed constant ((rad/s)/V). */ + double Kt; /* Torque constant (N.m/A). */ + double Rf; /* Bearing friction (N.m/(rad/s)). */ + double R; /* Terminal resistance (Ohm). */ + double L; /* Terminal inductance (H). */ + /* Gearbox characteristics. */ + double i_G; /* Gearbox ratio. */ + 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. */ +}; +typedef struct motor_model_def_t motor_model_def_t; + +/** Motor and load characteristics and current data. Angular speed and theta + * are at motor output, not gearbox output. */ +struct motor_model_t +{ + /* Motor and load characteristics. */ + struct motor_model_def_t m; + /* Simulation parameters. */ + double h; /* Simulation time step (s). */ + int d; /* Simulation time step division. */ + /* Simulation current state. */ + double t; /* Current time (not really used) (s). */ + double u; /* Current input voltage (V). */ + double i; /* Current current (A). */ + double o; /* Current angular speed (o for omega) (rad/s). */ + double th; /* Current theta (th for theta) (rad). */ +}; +typedef struct motor_model_t motor_model_t; + +/** Make a simulation step. */ +void motor_model_step (motor_model_t *m); + +#endif /* motor_model_host_h */ diff --git a/digital/avr/modules/motor/motor_model/motor_model_defs.host.c b/digital/avr/modules/motor/motor_model/motor_model_defs.host.c new file mode 100644 index 00000000..fddb16df --- /dev/null +++ b/digital/avr/modules/motor/motor_model/motor_model_defs.host.c @@ -0,0 +1,148 @@ +/* motor_model_defs.host.c */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "motor_model_defs.host.h" + +#include +#include + +/* RE25CLL with 1:10 gearbox. */ +const motor_model_def_t motor_model_def_re25cll_x10 = +{ + /* Motor characteristics. */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.18, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox characteristics. */ + 10, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + +/* RE25G with 1:20.25 gearbox. */ +const motor_model_def_t motor_model_def_re25g_x20_25 = +{ + /* Motor characteristics. */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.32, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox characteristics. */ + 20.25, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + +/* AMAX32GHP with 1:16 gearbox. */ +const motor_model_def_t motor_model_def_amax32ghp_x16 = +{ + /* Motor characteristics. */ + 269 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 25.44 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 3.99, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox characteristics. */ + 16, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + +/* Faulhaber 2657 with 1:9.7 gearbox. */ +const motor_model_def_t motor_model_def_faulhaber_2657_x9_7 = +{ + /* Motor characteristics. */ + 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 34.8 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.84, /* Terminal resistance (Ohm). */ + 0.380 / 1000, /* Terminal inductance (H). */ + /* Gearbox characteristics. */ + 9.7, /* Gearbox ratio. */ + 0.80, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + +/* Faulhaber 2342 with 23/1 1:3.71 gearbox. */ +const motor_model_def_t motor_model_def_faulhaber_2342_x3_71 = +{ + /* Motor characteristics. */ + 366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 26.10 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 7.10, /* Terminal resistance (Ohm). */ + 0.265 / 1000, /* Terminal inductance (H). */ + /* Gearbox characteristics. */ + 3.71, /* Gearbox ratio. */ + 0.88, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + +struct motor_model_def_table_t +{ + const char *name; + const motor_model_def_t *def; +}; + +static const struct motor_model_def_table_t def_table[] = +{ + { "re25cll_x10", &motor_model_def_re25cll_x10 }, + { "re25g_x20_25", &motor_model_def_re25g_x20_25 }, + { "amax32ghp_x16", &motor_model_def_amax32ghp_x16 }, + { "faulhaber_2657_x9_7", &motor_model_def_faulhaber_2657_x9_7 }, + { "faulhaber_2342_x3_71", &motor_model_def_faulhaber_2342_x3_71 }, + { NULL, NULL } +}; + +const motor_model_def_t * +motor_model_def_get (const char *name) +{ + const struct motor_model_def_table_t *p = def_table; + for (p = def_table; p->name; p++) + { + if (strcmp (p->name, name) == 0) + return p->def; + } + return NULL; +} + diff --git a/digital/avr/modules/motor/motor_model/motor_model_defs.host.h b/digital/avr/modules/motor/motor_model/motor_model_defs.host.h new file mode 100644 index 00000000..24d3a4a0 --- /dev/null +++ b/digital/avr/modules/motor/motor_model/motor_model_defs.host.h @@ -0,0 +1,40 @@ +#ifndef motor_model_defs_host_h +#define motor_model_defs_host_h +/* motor_model_defs.host.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +#include "motor_model.host.h" + +extern const motor_model_def_t motor_model_def_re25cll_x10; +extern const motor_model_def_t motor_model_def_re25g_x20_25; +extern const motor_model_def_t motor_model_def_amax32ghp_x16; +extern const motor_model_def_t motor_model_def_faulhaber_2657_x9_7; +extern const motor_model_def_t motor_model_def_faulhaber_2342_x3_71; + +/** Get a motor model definition by its name. */ +const motor_model_def_t * +motor_model_def_get (const char *name); + +#endif /* motor_model_defs_host_h */ diff --git a/digital/avr/modules/motor/motor_model/test/Makefile b/digital/avr/modules/motor/motor_model/test/Makefile new file mode 100644 index 00000000..256d1128 --- /dev/null +++ b/digital/avr/modules/motor/motor_model/test/Makefile @@ -0,0 +1,10 @@ +BASE = ../../../.. +HOST_PROGS = test_motor_model +test_motor_model_SOURCES = test_motor_model.c +MODULES = motor/motor_model +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 +HOST_LIBS = -lm + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/motor/motor_model/test/test_motor_model.c b/digital/avr/modules/motor/motor_model/test/test_motor_model.c new file mode 100644 index 00000000..e100a222 --- /dev/null +++ b/digital/avr/modules/motor/motor_model/test/test_motor_model.c @@ -0,0 +1,79 @@ +/* test_motor_model.c - Test DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "modules/motor/motor_model/motor_model_defs.host.h" + +#include +#include +#include + +void +simu (motor_model_t *m, double t) +{ + int i, s; + s = t / m->h; + for (i = 0; i < s; i++) + { + motor_model_step (m); + printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th); + } +} + +int +main (int argc, char **argv) +{ + double u_max, load; + motor_model_t ms, *m; + const motor_model_def_t *md; + /* Check arguments. */ + if (argc != 4) + { + fprintf (stderr, "syntax: %s MODEL UMAX LOAD\n", argv[0]); + return 1; + } + u_max = atof (argv[2]); + load = atof (argv[3]); + md = motor_model_def_get (argv[1]); + if (!md) + { + fprintf (stderr, "model unknown\n"); + return 1; + } + m = &ms; + memset (m, 0, sizeof (*m)); + m->m = *md; + m->m.J = load; + m->h = 1e-3; + m->d = 1000; + /* Make a step response simulation. */ + printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta"); + m->u = u_max; + printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th); + simu (m, 1.0); + m->u = 0.0; + simu (m, 1.0); + return 0; +} + -- cgit v1.2.3