summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2011-12-18 15:16:09 +0100
committerNicolas Schodet2012-03-06 23:16:34 +0100
commit5948ad3b390c3c7a8deac3fea4aa6f6e93f395f7 (patch)
tree79c69b4f1ab064751c03a6078494119c6a2eb375
parent928dc853e088849580aac368401999ea596644bb (diff)
digital/avr/modules/motor: add motor model
-rw-r--r--digital/avr/modules/motor/motor_model/Makefile.module1
-rw-r--r--digital/avr/modules/motor/motor_model/motor_model.host.c92
-rw-r--r--digital/avr/modules/motor/motor_model/motor_model.host.h69
-rw-r--r--digital/avr/modules/motor/motor_model/motor_model_defs.host.c148
-rw-r--r--digital/avr/modules/motor/motor_model/motor_model_defs.host.h40
-rw-r--r--digital/avr/modules/motor/motor_model/test/Makefile10
-rw-r--r--digital/avr/modules/motor/motor_model/test/test_motor_model.c79
7 files changed, 439 insertions, 0 deletions
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 <math.h>
+#include <string.h>
+
+/* 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+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;
+}
+