From 19db408880b2b607db88053f24e08779d632f3ca Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 22 Apr 2006 18:52:41 +0000 Subject: Ajout de la simulation des capteurs RVB. --- n/asserv/src/asserv/Makefile | 1 + n/asserv/src/asserv/models.host.c | 6 +- n/asserv/src/asserv/simu.host.c | 116 +++++++++++++++++++++++++++++++-- n/asserv/src/asserv/test_motor_model.c | 4 +- 4 files changed, 115 insertions(+), 12 deletions(-) diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile index 7ed24d9..023a8fd 100644 --- a/n/asserv/src/asserv/Makefile +++ b/n/asserv/src/asserv/Makefile @@ -11,5 +11,6 @@ AVR_MCU = atmega128 # -O2 : speed # -Os : size OPTIMIZE = -O2 +HOST_LIBS = -lm include $(BASE)/make/Makefile.gen diff --git a/n/asserv/src/asserv/models.host.c b/n/asserv/src/asserv/models.host.c index 15e7259..f978cb6 100644 --- a/n/asserv/src/asserv/models.host.c +++ b/n/asserv/src/asserv/models.host.c @@ -57,7 +57,7 @@ static const struct motor_t gloubi_model = static const struct robot_t gloubi_robot = { - gloubi_model, + &gloubi_model, 26.0, /* Distance between the wheels (m). */ }; @@ -90,7 +90,7 @@ static const struct motor_t taz_model = static const struct robot_t taz_robot = { - taz_model, + &taz_model, 30.0, /* Distance between the wheels (m). */ }; @@ -123,7 +123,7 @@ static const struct motor_t tazg_model = static const struct robot_t tazg_robot = { - tazg_model, + &tazg_model, 30.0, /* Distance between the wheels (m). */ }; diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c index 01020d1..1f47de5 100644 --- a/n/asserv/src/asserv/simu.host.c +++ b/n/asserv/src/asserv/simu.host.c @@ -23,6 +23,7 @@ * }}} */ #include "common.h" #include "modules/host/host.h" +#include "modules/utils/utils.h" #include "simu.host.h" #include "motor_model.host.h" #include "models.host.h" @@ -32,7 +33,7 @@ #include /** Simulate some AVR regs. */ -uint8_t DDRD, PORTD, PORTA, PORTC, PINA; +uint8_t DDRD, PORTD, PORTA, PORTC, PINA, PINF; /** Overall counter values. */ uint16_t counter_left, counter_right; @@ -50,7 +51,62 @@ uint8_t pwm_dir; struct motor_t simu_left_model, simu_right_model; /** Computed simulated position. */ -double simu_pos_x, simu_pos_y; +double simu_pos_x, simu_pos_y, simu_pos_a; + +/** Distance between wheels. */ +double simu_footing; + +/** Holes positions. */ +#define SIMU_HOLES_RADIUS 5 +struct simu_hole_t +{ + double x, y; + int color; +}; +static const struct simu_hole_t simu_holes[] = +{ + { 1050, 1500 - 990, 0 }, + { 1050 - 360, 1500 - 920, 0 }, + { 1050 + 360, 1500 - 920, 0 }, + { 1050, 1500 - 690, 0 }, + { 1050 - 680, 1500 - 600, 0 }, + { 1050 + 680, 1500 - 600, 0 }, + { 1050 - 300, 1500 - 570, 0 }, + { 1050 + 300, 1500 - 570, 0 }, + { 1050 - 500, 1500 - 270, 0 }, + { 1050 + 500, 1500 - 270, 1 }, + { 1050 - 860, 1500 - 160, 1 }, + { 1050 - 160, 1500 - 160, 1 }, + { 1050 + 160, 1500 - 160, 0 }, + { 1050 + 860, 1500 - 160, 0 }, + { 1050 + 860, 1500 + 160, 1 }, + { 1050 + 160, 1500 + 160, 1 }, + { 1050 - 160, 1500 + 160, 0 }, + { 1050 - 860, 1500 + 160, 0 }, + { 1050 + 500, 1500 + 270, 0 }, + { 1050 - 500, 1500 + 270, 1 }, + { 1050 + 300, 1500 + 570, 1 }, + { 1050 - 300, 1500 + 570, 1 }, + { 1050 + 680, 1500 + 600, 1 }, + { 1050 - 680, 1500 + 600, 1 }, + { 1050, 1500 + 690, 0 }, + { 1050 + 360, 1500 + 920, 1 }, + { 1050 - 360, 1500 + 920, 1 }, + { 1050, 1500 + 990, 0 }, +}; + +/** RGB detector positions. */ +struct simu_rgb_t +{ + double x, y; +}; +static const struct simu_rgb_t simu_rgbs[4] = +{ + { 0, 70 }, + { -5.5, 70 }, + { +5.5, 70 }, + { 0, 0 }, +}; /** Initialise simulation. */ static void @@ -58,7 +114,7 @@ simu_init (void) { int argc; char **argv; - const struct motor_t *m; + const struct robot_t *m; host_get_program_arguments (&argc, &argv); if (argc != 1) { @@ -71,8 +127,9 @@ simu_init (void) fprintf (stderr, "unknown model name: %s\n", argv[0]); exit (1); } - simu_left_model = *m; - simu_right_model = *m; + simu_left_model = *m->motor; + simu_right_model = *m->motor; + simu_footing = m->footing; simu_pos_x = simu_pos_y = 0; } @@ -80,9 +137,52 @@ simu_init (void) static void simu_pos_update (double dl, double dr) { - double d = 0.5 * (dl + dr); - double da; + double da = (dr - dl) / simu_footing; + double na = simu_pos_a + da; + if (da < 0.0001 && da > -0.0001) + { + /* Avoid a division by zero when angle is too small. */ + double a = simu_pos_a + da * 0.5; + simu_pos_x += d * cos (a); + simu_pos_y += d * sin (a); + } + else + { + /* Radius of turn is d / da. */ + simu_pos_x += (sin (na) - sin (simu_pos_a)) * d / da; + simu_pos_y += (cos (simu_pos_a) - cos (na)) * d / da; + } + simu_pos_a = na; +} + +/** Update RGB sensors. */ +static void +simu_rgb_update (void) +{ + unsigned i, j; + int rgb_x, rgb_y; + int dx, dy; + PINF &= 0xf0; + for (i = 0; i < UTILS_COUNT (simu_rgbs); i++) + { + /* Compute RGB sensor position in absolute coordinates. */ + rgb_x = simu_pos_x + cos (simu_pos_a) * simu_rgbs[i].x + - sin (simu_pos_a) * simu_rgbs[i].y; + rgb_y = simu_pos_y + sin (simu_pos_a) * simu_rgbs[i].x + + cos (simu_pos_a) * simu_rgbs[i].y; + /* Is it above any hole? */ + for (j = 0; j < UTILS_COUNT (simu_holes); j++) + { + dx = simu_holes[j].x - rgb_x; + dy = simu_holes[j].y - rgb_y; + if (dx * dx + dy * dy < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS) + { + PINF |= 1 << i; + break; + } + } + } } /** Do a simulation step. */ @@ -112,6 +212,8 @@ simu_step (void) * simu_left_model.i_G * simu_left_model.w_r, (simu_right_model.th - old_right_th) * simu_right_model.i_G * simu_right_model.w_r); + /* Update RGB sensors. */ + simu_rgb_update (); } /** Initialise the timer. */ diff --git a/n/asserv/src/asserv/test_motor_model.c b/n/asserv/src/asserv/test_motor_model.c index c166999..f5fb575 100644 --- a/n/asserv/src/asserv/test_motor_model.c +++ b/n/asserv/src/asserv/test_motor_model.c @@ -44,7 +44,7 @@ main (int argc, char **argv) { struct motor_t ms; struct motor_t *m; - const struct motor_t *mr; + const struct robot_t *mr; /* Check arguments. */ if (argc != 2) { @@ -57,7 +57,7 @@ main (int argc, char **argv) fprintf (stderr, "model unknown\n"); return 1; } - ms = *mr; + ms = *mr->motor; m = &ms; /* Make a step response simulation. */ printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta"); -- cgit v1.2.3