From 88f1c7071d15f867244c1bb31d3766e657c0a5c1 Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 19 Apr 2006 07:28:20 +0000 Subject: Début de simulation de positionnement. --- n/asserv/src/asserv/models.host.c | 36 ++++++++++++++++++++++++++++------ n/asserv/src/asserv/models.host.h | 8 +++++++- n/asserv/src/asserv/motor_model.host.h | 2 ++ n/asserv/src/asserv/simu.host.c | 18 +++++++++++++++++ 4 files changed, 57 insertions(+), 7 deletions(-) (limited to 'n/asserv') 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. */ -- cgit v1.2.3