summaryrefslogtreecommitdiff
path: root/n
diff options
context:
space:
mode:
authorschodet2006-03-11 12:33:39 +0000
committerschodet2006-03-11 12:33:39 +0000
commitc4395e067140b7bbc40a20c4d8d720d30b096033 (patch)
tree3242e9970d08f1a8937a04b5249c8da890b20e89 /n
parent2f3a905da7a450c200ea631ce9669e9258678d3a (diff)
Ajout de la simulation sur host avec simulation des moteurs.
Diffstat (limited to 'n')
-rw-r--r--n/asserv/src/asserv/Makefile4
-rw-r--r--n/asserv/src/asserv/main.c5
-rw-r--r--n/asserv/src/asserv/models.host.c130
-rw-r--r--n/asserv/src/asserv/models.host.h32
-rw-r--r--n/asserv/src/asserv/motor_model.host.c (renamed from n/asserv/src/asserv/motor_model.c)2
-rw-r--r--n/asserv/src/asserv/motor_model.host.h (renamed from n/asserv/src/asserv/motor_model.h)8
-rw-r--r--n/asserv/src/asserv/simu.host.c59
-rw-r--r--n/asserv/src/asserv/simu.host.h6
-rw-r--r--n/asserv/src/asserv/taz_model.c52
-rw-r--r--n/asserv/src/asserv/test_motor_model.c25
10 files changed, 253 insertions, 70 deletions
diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile
index 6fe0762..3dbda13 100644
--- a/n/asserv/src/asserv/Makefile
+++ b/n/asserv/src/asserv/Makefile
@@ -1,8 +1,8 @@
BASE = ../../../avr
PROGS = asserv
HOST_PROGS = test_motor_model
-asserv_SOURCES = main.c simu.host.c
-test_motor_model_SOURCES = test_motor_model.c motor_model.c taz_model.c
+asserv_SOURCES = main.c simu.host.c motor_model.host.c models.host.c
+test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c
MODULES = proto uart utils
test_motor_model_MODULES =
CONFIGFILE = avrconfig.h
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 87124cf..b32f2b1 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -36,12 +36,14 @@
# include "timer.avr.c"
# include "counter.avr.c"
# include "pwm.avr.c"
-# include "eeprom.avr.c"
#else
# include "simu.host.h"
#endif
#include "pos.c"
#include "speed.c"
+#ifndef HOST
+# include "eeprom.avr.c"
+#endif
/** Motor control mode:
* 0: pwm setup.
@@ -82,6 +84,7 @@ main_loop (void);
int
main (int argc, char **argv)
{
+ avr_init (argc, argv);
DDRD = 0x60;
/* Pull-ups. */
PORTA = 0xff;
diff --git a/n/asserv/src/asserv/models.host.c b/n/asserv/src/asserv/models.host.c
new file mode 100644
index 0000000..062b88a
--- /dev/null
+++ b/n/asserv/src/asserv/models.host.c
@@ -0,0 +1,130 @@
+/* models.host.c */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * 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"
+#include "models.host.h"
+
+#include <math.h>
+#include <string.h>
+
+/* Gloubi model. */
+static const struct motor_t gloubi_model =
+{
+ /* Motor caracteristics. */
+ 407 * M_2_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 caracteristics. */
+ 10, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load caracteristics. */
+ 4 * 0.02 * 0.02, /* Load (kg.m^2). */
+ /* Simulation parameters. */
+ 4.444444 / 1000, /* Simulation time step (s). */
+ 1000, /* Simulation time step division. */
+ /* Simulation current state. */
+ 0, /* Current time (not realy used) (s). */
+ 0, /* Current input voltage (V). */
+ 0, /* Current current (A). */
+ 0, /* Current angular speed (o for omega) (rad/s). */
+ 0 /* Current theta (th for theta) (rad). */
+};
+
+/* Taz model. */
+static const struct motor_t taz_model =
+{
+ /* Motor caracteristics. */
+ 407 * M_2_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 caracteristics. */
+ 10, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load caracteristics. */
+ 10 * 0.04 * 0.04, /* Load (kg.m^2). */
+ /* Simulation parameters. */
+ 4.444444 / 1000, /* Simulation time step (s). */
+ 1000, /* Simulation time step division. */
+ /* Simulation current state. */
+ 0, /* Current time (not realy used) (s). */
+ 0, /* Current input voltage (V). */
+ 0, /* Current current (A). */
+ 0, /* Current angular speed (o for omega) (rad/s). */
+ 0 /* Current theta (th for theta) (rad). */
+};
+
+/* Taz model, with a RE25G and a 1:20.25 ratio gearbox. */
+static const struct motor_t tazg_model =
+{
+ /* Motor caracteristics. */
+ 407 * M_2_PI / 60, /* Speed constant ((rad/s)/V). */
+ 40.2 / 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 caracteristics. */
+ 20.25, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load caracteristics. */
+ 10 * 0.04 * 0.04, /* Load (kg.m^2). */
+ /* Simulation parameters. */
+ 4.444444 / 1000, /* Simulation time step (s). */
+ 1000, /* Simulation time step division. */
+ /* Simulation current state. */
+ 0, /* Current time (not realy used) (s). */
+ 0, /* Current input voltage (V). */
+ 0, /* Current current (A). */
+ 0, /* Current angular speed (o for omega) (rad/s). */
+ 0 /* Current theta (th for theta) (rad). */
+};
+
+/* Table of models. */
+static const struct
+{
+ const char *name;
+ const struct motor_t *model;
+} models[] = {
+ { "gloubi", &gloubi_model },
+ { "taz", &taz_model },
+ { "tazg", &tazg_model },
+ { 0, 0 }
+};
+
+/** Get a pointer to a model by name, or return 0. */
+const struct motor_t *
+models_get (const char *name)
+{
+ int i;
+ for (i = 0; models[i].name; i++)
+ {
+ if (strcmp (models[i].name, name) == 0)
+ return models[i].model;
+ }
+ return 0;
+}
+
diff --git a/n/asserv/src/asserv/models.host.h b/n/asserv/src/asserv/models.host.h
new file mode 100644
index 0000000..b962b9d
--- /dev/null
+++ b/n/asserv/src/asserv/models.host.h
@@ -0,0 +1,32 @@
+#ifndef models_host_h
+#define models_host_h
+/* models.host.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2006.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * 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.
+ *
+ * }}} */
+
+/** Get a pointer to a model by name, or return 0. */
+const struct motor_t *
+models_get (const char *name);
+
+#endif /* models_host_h */
diff --git a/n/asserv/src/asserv/motor_model.c b/n/asserv/src/asserv/motor_model.host.c
index bff5cad..1d0855b 100644
--- a/n/asserv/src/asserv/motor_model.c
+++ b/n/asserv/src/asserv/motor_model.host.c
@@ -23,7 +23,7 @@
*
* }}} */
#include "common.h"
-#include "motor_model.h"
+#include "motor_model.host.h"
/**
* Switching to french for all thoses non english speaking people.
diff --git a/n/asserv/src/asserv/motor_model.h b/n/asserv/src/asserv/motor_model.host.h
index a7446e1..e3ab615 100644
--- a/n/asserv/src/asserv/motor_model.h
+++ b/n/asserv/src/asserv/motor_model.host.h
@@ -1,6 +1,6 @@
-#ifndef motor_model_h
-#define motor_model_h
-/* motor_model.h - DC motor model. */
+#ifndef motor_model_host_h
+#define motor_model_host_h
+/* motor_model.host.h - DC motor model. */
/* asserv - Position & speed motor control on AVR. {{{
*
* Copyright (C) 2006 Nicolas Schodet
@@ -53,4 +53,4 @@ struct motor_t
/** Make a simulation step. */
void motor_model_step (struct motor_t *m);
-#endif /* motor_model_h */
+#endif /* motor_model_host_h */
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 5b2b6d4..e7cc4ee 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -22,7 +22,14 @@
* Email: <contact@ni.fr.eu.org>
* }}} */
#include "common.h"
+#include "modules/host/host.h"
#include "simu.host.h"
+#include "motor_model.host.h"
+#include "models.host.h"
+
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
/** Simulate some AVR regs. */
uint8_t DDRD, PORTD, PORTA, PORTC, PINA;
@@ -40,19 +47,67 @@ int16_t pwm_left, pwm_right;
* on port B. */
uint8_t pwm_dir;
-/* +AutoDec */
-/* -AutoDec */
+struct motor_t simu_left_model, simu_right_model;
+
+/** Initialise simulation. */
+static void
+simu_init (void)
+{
+ int argc;
+ char **argv;
+ const struct motor_t *m;
+ host_get_program_arguments (&argc, &argv);
+ if (argc != 1)
+ {
+ fprintf (stderr, "need model name as first argument\n");
+ exit (1);
+ }
+ m = models_get (argv[0]);
+ if (!m)
+ {
+ fprintf (stderr, "unknown model name: %s\n", argv[0]);
+ exit (1);
+ }
+ simu_left_model = *m;
+ simu_right_model = *m;
+}
+
+/** Do a simulation step. */
+static void
+simu_step (void)
+{
+ double old_left_th, old_right_th;
+ /* Convert pwm value into voltage. */
+ assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX);
+ assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX);
+ simu_left_model.u = (double) pwm_left / PWM_MAX;
+ simu_right_model.u = (double) pwm_right / PWM_MAX;
+ /* Make one step. */
+ old_left_th = simu_left_model.th;
+ old_right_th = simu_right_model.th;
+ motor_model_step (&simu_left_model);
+ motor_model_step (&simu_right_model);
+ /* Modifie counters. */
+ counter_left_diff = (simu_left_model.th - old_left_th) / M_2_PI * 500 *
+ simu_left_model.i_G;
+ counter_left += counter_left_diff;
+ counter_right_diff = (simu_right_model.th - old_right_th) / M_2_PI * 500 *
+ simu_right_model.i_G;
+ counter_right += counter_right_diff;
+}
/** Initialise the timer. */
void
timer_init (void)
{
+ simu_init ();
}
/** Wait for timer overflow. */
void
timer_wait (void)
{
+ simu_step ();
}
/** Read timer value. Used for performance analysis. */
diff --git a/n/asserv/src/asserv/simu.host.h b/n/asserv/src/asserv/simu.host.h
index c2a4934..11f817c 100644
--- a/n/asserv/src/asserv/simu.host.h
+++ b/n/asserv/src/asserv/simu.host.h
@@ -49,9 +49,9 @@ extern int16_t pwm_left, pwm_right;
extern uint8_t pwm_dir;
#define EEPROM_KEY 0xa5
-#define eeprom_read_params()
-#define eeprom_write_params()
-#define eeprom_clear_params()
+#define eeprom_read_params() do { } while (0)
+#define eeprom_write_params() do { } while (0)
+#define eeprom_clear_params() do { } while (0)
/* +AutoDec */
diff --git a/n/asserv/src/asserv/taz_model.c b/n/asserv/src/asserv/taz_model.c
deleted file mode 100644
index a590029..0000000
--- a/n/asserv/src/asserv/taz_model.c
+++ /dev/null
@@ -1,52 +0,0 @@
-/* taz_model.c */
-/* asserv - Position & speed motor control on AVR. {{{
- *
- * Copyright (C) 2006 Nicolas Schodet
- *
- * 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.
- *
- * Contact :
- * Web: http://perso.efrei.fr/~schodet/
- * Email: <contact@ni.fr.eu.org>
- * }}} */
-#include "motor_model.h"
-
-#include <math.h>
-
-/* Taz model. */
-struct motor_t taz_model =
-{
- /* Motor caracteristics. */
- 407 * M_2_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 caracteristics. */
- 10, /* Gearbox ratio. */
- 0.75, /* Gearbox efficiency. */
- /* Load caracteristics. */
- 10 * 0.04 * 0.04, /* Load (kg.m^2). */
- /* Simulation parameters. */
- 4.444444 / 1000, /* Simulation time step (s). */
- 1000, /* Simulation time step division. */
- /* Simulation current state. */
- 0, /* Current time (not realy used) (s). */
- 0, /* Current input voltage (V). */
- 0, /* Current current (A). */
- 0, /* Current angular speed (o for omega) (rad/s). */
- 0 /* Current theta (th for theta) (rad). */
-};
-
diff --git a/n/asserv/src/asserv/test_motor_model.c b/n/asserv/src/asserv/test_motor_model.c
index a478244..c166999 100644
--- a/n/asserv/src/asserv/test_motor_model.c
+++ b/n/asserv/src/asserv/test_motor_model.c
@@ -23,12 +23,11 @@
*
* }}} */
#include "common.h"
-#include "motor_model.h"
+#include "motor_model.host.h"
+#include "models.host.h"
#include <stdio.h>
-extern struct motor_t taz_model;
-
void simu (struct motor_t *m, double t)
{
int i, s;
@@ -41,9 +40,25 @@ void simu (struct motor_t *m, double t)
}
int
-main (void)
+main (int argc, char **argv)
{
- struct motor_t *m = &taz_model;
+ struct motor_t ms;
+ struct motor_t *m;
+ const struct motor_t *mr;
+ /* Check arguments. */
+ if (argc != 2)
+ {
+ fprintf (stderr, "syntax: %s MODEL\n", argv[0]);
+ return 1;
+ }
+ mr = models_get (argv[1]);
+ if (!mr)
+ {
+ fprintf (stderr, "model unknown\n");
+ return 1;
+ }
+ ms = *mr;
+ m = &ms;
/* Make a step response simulation. */
printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta");
m->u = 3.0;