summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
authorschodet2006-04-22 18:52:41 +0000
committerschodet2006-04-22 18:52:41 +0000
commit19db408880b2b607db88053f24e08779d632f3ca (patch)
treef312bf61c0b96e6e0e8132c8d9d67035bfa6c13d /n/asserv/src
parentcc67b62122a3e26a22c1273e69546ef2f81e974d (diff)
Ajout de la simulation des capteurs RVB.
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/asserv/Makefile1
-rw-r--r--n/asserv/src/asserv/models.host.c6
-rw-r--r--n/asserv/src/asserv/simu.host.c116
-rw-r--r--n/asserv/src/asserv/test_motor_model.c4
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 <stdio.h>
/** 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");