summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2006-04-19 07:28:20 +0000
committerschodet2006-04-19 07:28:20 +0000
commit88f1c7071d15f867244c1bb31d3766e657c0a5c1 (patch)
treecf71ba78142c0b4e6988c3d628f2d21b28fa6ae3
parentc79e050bdf5f6a208341eee307c58a2807427c65 (diff)
Début de simulation de positionnement.
-rw-r--r--n/asserv/src/asserv/models.host.c36
-rw-r--r--n/asserv/src/asserv/models.host.h8
-rw-r--r--n/asserv/src/asserv/motor_model.host.h2
-rw-r--r--n/asserv/src/asserv/simu.host.c18
4 files changed, 57 insertions, 7 deletions
diff --git a/n/asserv/src/asserv/models.host.c b/n/asserv/src/asserv/models.host.c
index 062b88a..15e7259 100644
--- a/n/asserv/src/asserv/models.host.c
+++ b/n/asserv/src/asserv/models.host.c
@@ -42,6 +42,8 @@ static const struct motor_t gloubi_model =
0.75, /* Gearbox efficiency. */
/* Load caracteristics. */
4 * 0.02 * 0.02, /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ 0.02, /* Wheel radius (m). */
/* Simulation parameters. */
4.444444 / 1000, /* Simulation time step (s). */
1000, /* Simulation time step division. */
@@ -53,6 +55,12 @@ static const struct motor_t gloubi_model =
0 /* Current theta (th for theta) (rad). */
};
+static const struct robot_t gloubi_robot =
+{
+ gloubi_model,
+ 26.0, /* Distance between the wheels (m). */
+};
+
/* Taz model. */
static const struct motor_t taz_model =
{
@@ -67,6 +75,8 @@ static const struct motor_t taz_model =
0.75, /* Gearbox efficiency. */
/* Load caracteristics. */
10 * 0.04 * 0.04, /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ 0.04, /* Wheel radius (m). */
/* Simulation parameters. */
4.444444 / 1000, /* Simulation time step (s). */
1000, /* Simulation time step division. */
@@ -78,6 +88,12 @@ static const struct motor_t taz_model =
0 /* Current theta (th for theta) (rad). */
};
+static const struct robot_t taz_robot =
+{
+ taz_model,
+ 30.0, /* Distance between the wheels (m). */
+};
+
/* Taz model, with a RE25G and a 1:20.25 ratio gearbox. */
static const struct motor_t tazg_model =
{
@@ -92,6 +108,8 @@ static const struct motor_t tazg_model =
0.75, /* Gearbox efficiency. */
/* Load caracteristics. */
10 * 0.04 * 0.04, /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ 0.04, /* Wheel radius (m). */
/* Simulation parameters. */
4.444444 / 1000, /* Simulation time step (s). */
1000, /* Simulation time step division. */
@@ -103,27 +121,33 @@ static const struct motor_t tazg_model =
0 /* Current theta (th for theta) (rad). */
};
+static const struct robot_t tazg_robot =
+{
+ tazg_model,
+ 30.0, /* Distance between the wheels (m). */
+};
+
/* Table of models. */
static const struct
{
const char *name;
- const struct motor_t *model;
+ const struct robot_t *robot;
} models[] = {
- { "gloubi", &gloubi_model },
- { "taz", &taz_model },
- { "tazg", &tazg_model },
+ { "gloubi", &gloubi_robot },
+ { "taz", &taz_robot },
+ { "tazg", &tazg_robot },
{ 0, 0 }
};
/** Get a pointer to a model by name, or return 0. */
-const struct motor_t *
+const struct robot_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 models[i].robot;
}
return 0;
}
diff --git a/n/asserv/src/asserv/models.host.h b/n/asserv/src/asserv/models.host.h
index b962b9d..4ae55ad 100644
--- a/n/asserv/src/asserv/models.host.h
+++ b/n/asserv/src/asserv/models.host.h
@@ -25,8 +25,14 @@
*
* }}} */
+struct robot_t
+{
+ const struct motor_t *motor;
+ double footing; /* Distance between the wheels (m). */
+};
+
/** Get a pointer to a model by name, or return 0. */
-const struct motor_t *
+const struct robot_t *
models_get (const char *name);
#endif /* models_host_h */
diff --git a/n/asserv/src/asserv/motor_model.host.h b/n/asserv/src/asserv/motor_model.host.h
index e3ab615..931890a 100644
--- a/n/asserv/src/asserv/motor_model.host.h
+++ b/n/asserv/src/asserv/motor_model.host.h
@@ -39,6 +39,8 @@ struct motor_t
double ro_G;/* Gearbox efficiency. */
/* Load caracteristics. */
double J; /* Load (kg.m^2). */
+ /* Wheel caracteristics. */
+ double w_r; /* Wheel radius (m). */
/* Simulation parameters. */
double h; /* Simulation time step (s). */
int d; /* Simulation time step division. */
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 1035af1..01020d1 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -49,6 +49,9 @@ uint8_t pwm_dir;
struct motor_t simu_left_model, simu_right_model;
+/** Computed simulated position. */
+double simu_pos_x, simu_pos_y;
+
/** Initialise simulation. */
static void
simu_init (void)
@@ -70,6 +73,16 @@ simu_init (void)
}
simu_left_model = *m;
simu_right_model = *m;
+ simu_pos_x = simu_pos_y = 0;
+}
+
+/** Update simulation position. */
+static void
+simu_pos_update (double dl, double dr)
+{
+
+ double d = 0.5 * (dl + dr);
+ double da;
}
/** Do a simulation step. */
@@ -94,6 +107,11 @@ simu_step (void)
counter_right_diff = (simu_right_model.th - old_right_th) / M_2_PI * 500 *
simu_right_model.i_G;
counter_right += counter_right_diff;
+ /* Update position */
+ simu_pos_update ((simu_left_model.th - old_left_th)
+ * 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);
}
/** Initialise the timer. */