From 67ef08749924e759a04834d27cee40ec6e0a1854 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 16 Mar 2008 18:36:49 +0100 Subject: * digital/asserv/src/asserv: - added AMAX32GHP model. - fixed missing voltage multiplication. - added support for separated encoders. - model reorganisation. - fixed counters drift. --- digital/asserv/src/asserv/models.host.c | 228 ++++++++++++++++----------- digital/asserv/src/asserv/models.host.h | 34 +++- digital/asserv/src/asserv/motor_model.host.c | 14 +- digital/asserv/src/asserv/motor_model.host.h | 25 +-- digital/asserv/src/asserv/simu.host.c | 92 +++++++---- digital/asserv/src/asserv/test_motor_model.c | 4 +- 6 files changed, 256 insertions(+), 141 deletions(-) diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index 7436ed43..024e592c 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -28,140 +28,159 @@ #include #include -/* Gloubi model. */ -static const struct motor_t gloubi_model = +/* RE25CLL with 1:10 gearbox model. */ +static const struct motor_def_t re25cll_model = { - /* Motor caracteristics. */ + /* Motor characteristics. */ 407 * (2*M_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. */ + 12.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ 10, /* Gearbox ratio. */ 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. */ - /* 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). */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ }; -static const struct robot_t gloubi_robot = +/* RE25G with 1:20.25 gearbox model. */ +static const struct motor_def_t re25g_model = { - &gloubi_model, - NULL, - 26.0, /* Distance between the wheels (m). */ + /* Motor characteristics. */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.32, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 20.25, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ }; -/* Taz model. */ -static const struct motor_t taz_model = +/* AMAX32GHP with 1:16 gearbox model. */ +static const struct motor_def_t amax32ghp_model = { - /* Motor caracteristics. */ - 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ - 23.4 / 1000, /* Torque constant (N.m/A). */ + /* Motor characteristics. */ + 269 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 25.44 / 1000, /* Torque constant (N.m/A). */ 0, /* Bearing friction (N.m/(rad/s)). */ - 2.18, /* Terminal resistance (Ohm). */ + 3.99, /* Terminal resistance (Ohm). */ 0.24 / 1000, /* Terminal inductance (H). */ - /* Gearbox caracteristics. */ - 10, /* Gearbox ratio. */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 16, /* Gearbox ratio. */ 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. */ - /* 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). */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ }; -static const struct robot_t taz_robot = +/* Gloubi, Efrei 2006. */ +static const struct robot_t gloubi_robot = { - &taz_model, - NULL, - 30.0, /* Distance between the wheels (m). */ + /* Main motors. */ + &re25cll_model, + /* Number of steps on the main motors encoders. */ + 500, + /* Wheel radius (m). */ + 0.02, + /* Distance between the wheels (m). */ + 0.26, + /* Weight of the robot (kg). */ + 4.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 13.0, // approx + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 0, + 0.0, 0.0, NULL, 0 }; -/* Taz model, with a RE25G and a 1:20.25 ratio gearbox. */ -static const struct motor_t tazg_model = +/* Taz, APBTeam/Efrei 2005. */ +static const struct robot_t taz_robot = { - /* Motor caracteristics. */ - 407 * (2*M_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). */ - /* Wheel caracteristics. */ - 0.04, /* Wheel radius (m). */ - /* 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). */ + /* Main motors. */ + &re25cll_model, + /* Number of steps on the main motors encoders. */ + 500, + /* Wheel radius (m). */ + 0.04, + /* Distance between the wheels (m). */ + 0.30, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.0, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 0, + 0.0, 0.0, NULL, 0 }; +/* TazG, Taz with RE25G motors. */ static const struct robot_t tazg_robot = { - &tazg_model, - NULL, - 30.0, /* Distance between the wheels (m). */ + /* Main motors. */ + &re25g_model, + /* Number of steps on the main motors encoders. */ + 500, + /* Wheel radius (m). */ + 0.04, + /* Distance between the wheels (m). */ + 0.30, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.0, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 0, + 0.0, 0.0, NULL, 0 }; /* Gerard arm model, with a RE25CLL and a 1:10 ratio gearbox. */ -static const struct motor_t gerard_arm_model = +static const struct motor_def_t gerard_arm_model = { - /* Motor caracteristics. */ + /* Motor characteristics. */ 407 * (2*M_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. */ + 12.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ 10, /* Gearbox ratio. */ 0.75, /* Gearbox efficiency. */ - /* Load caracteristics. */ + /* Load characteristics. */ 0.200 * 0.1 * 0.1, /* Load (kg.m^2). */ - /* Wheel caracteristics. */ - 0, /* Wheel radius (m). */ - /* 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). */ }; +/* Gerard, find it a name! */ static const struct robot_t gerard_robot = { - &tazg_model, + /* Main motors. */ + &amax32ghp_model, + /* Number of steps on the main motors encoders. */ + 5000, + /* Wheel radius (m). */ + 0.065 / 2, + /* Distance between the wheels (m). */ + 0.16, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.10, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 1, + /** Encoder wheel radius (m). */ + 0.063 / 2, + /** Distance between the encoders wheels (m). */ + 0.28, + /** First auxiliary motor or NULL if none. */ &gerard_arm_model, - 30.0, /* Distance between the wheels (m). */ + /** Number of steps on the first auxiliary motor encoder. */ + 500, }; /* Table of models. */ @@ -190,3 +209,32 @@ models_get (const char *name) return 0; } +/** Initialise simulation models. */ +void +models_init (const struct robot_t *robot, struct motor_t *main_motor_left, + struct motor_t *main_motor_right, struct motor_t *aux0_motor) +{ + if (main_motor_left) + { + main_motor_left->m = *robot->main_motor; + main_motor_left->m.J = robot->weight * robot->wheel_r + * robot->wheel_r / 2; + main_motor_left->h = ECHANT_PERIOD; + main_motor_left->d = 1000; + } + if (main_motor_right) + { + main_motor_right->m = *robot->main_motor; + main_motor_right->m.J = robot->weight * robot->wheel_r + * robot->wheel_r / 2; + main_motor_right->h = ECHANT_PERIOD; + main_motor_right->d = 1000; + } + if (aux0_motor && robot->aux0_motor) + { + aux0_motor->m = *robot->aux0_motor; + aux0_motor->h = ECHANT_PERIOD; + aux0_motor->d = 1000; + } +} + diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index a63a2776..36099e64 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -25,15 +25,43 @@ * * }}} */ +#define ECHANT_PERIOD (1.0 / (14745600.0 / 256 / 256)) + +/** Define a robot and its peripherals. + * Encoder characteristics are defined at gearbox output. */ struct robot_t { - const struct motor_t *motor; - const struct motor_t *aux0; - double footing; /* Distance between the wheels (m). */ + /** Main motors. */ + const struct motor_def_t *main_motor; + /** Number of steps on the main motors encoders. */ + int main_encoder_steps; + /** Wheel radius (m). */ + double wheel_r; + /** Distance between the wheels (m). */ + double footing; + /** Weight of the robot (kg). */ + double weight; + /** Distance of the gravity center from the center of motors axis (m). */ + double gravity_center_distance; + /** Whether the encoder is mounted on the main motor (false) or not (true). */ + int encoder_separated; + /** Encoder wheel radius (m). */ + double encoder_wheel_r; + /** Distance between the encoders wheels (m). */ + double encoder_footing; + /** First auxiliary motor or NULL if none. */ + const struct motor_def_t *aux0_motor; + /** Number of steps on the first auxiliary motor encoder. */ + int aux0_encoder_steps; }; /** Get a pointer to a model by name, or return 0. */ const struct robot_t * models_get (const char *name); +/** Initialise simulation models. */ +void +models_init (const struct robot_t *robot, struct motor_t *main_motor_left, + struct motor_t *main_motor_right, struct motor_t *aux0_motor); + #endif /* models_host_h */ diff --git a/digital/asserv/src/asserv/motor_model.host.c b/digital/asserv/src/asserv/motor_model.host.c index 9447c1c3..37171c32 100644 --- a/digital/asserv/src/asserv/motor_model.host.c +++ b/digital/asserv/src/asserv/motor_model.host.c @@ -26,7 +26,7 @@ #include "motor_model.host.h" /** - * Switching to french for all thoses non english speaking people. + * Switching to french for all those non english speaking people. * * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux * parties, la modélisation électrique et la modélisation mécanique. @@ -68,14 +68,14 @@ void motor_model_step (struct motor_t *m) /* Make several small steps to increase precision. */ for (; d; d--) { - /* Ah, the mistical power of computation. */ + /* Ah, the mystical power of computation. */ i_ = m->i - + h * (1.0 / m->L * m->u - - m->R / m->L * m->i - - 1.0 / m->Ke / m->L * m->o); + + h * (1.0 / m->m.L * m->u + - m->m.R / m->m.L * m->i + - 1.0 / m->m.Ke / m->m.L * m->o); o_ = m->o - + h * m->i_G * m->i_G * m->ro_G / m->J - * (m->Kt * m->i - m->Rf * m->o); + + h * m->m.i_G * m->m.i_G * m->m.ro_G / m->m.J + * (m->m.Kt * m->i - m->m.Rf * m->o); th_ = m->th + h * m->o; /* Ok, now store this step. */ m->i = i_; diff --git a/digital/asserv/src/asserv/motor_model.host.h b/digital/asserv/src/asserv/motor_model.host.h index 9b0ce712..3a04e15a 100644 --- a/digital/asserv/src/asserv/motor_model.host.h +++ b/digital/asserv/src/asserv/motor_model.host.h @@ -25,27 +25,34 @@ * * }}} */ -/** Motor and load caracteristics and current data. */ -struct motor_t +/** Motor and load characteristics. */ +struct motor_def_t { - /* Motor caracteristics. */ + /* Motor characteristics. */ double Ke; /* Speed constant ((rad/s)/V). */ double Kt; /* Torque constant (N.m/A). */ double Rf; /* Bearing friction (N.m/(rad/s)). */ double R; /* Terminal resistance (Ohm). */ double L; /* Terminal inductance (H). */ - /* Gearbox caracteristics. */ + double u_max;/* Maximum voltage (V). */ + /* Gearbox characteristics. */ double i_G; /* Gearbox ratio. */ double ro_G;/* Gearbox efficiency. */ - /* Load caracteristics. */ - double J; /* Load (kg.m^2). */ - /* Wheel caracteristics. */ - double w_r; /* Wheel radius (m). */ + /* Load characteristics. */ + double J; /* Load at gearbox output (kg.m^2). */ +}; + +/** Motor and load characteristics and current data. Angular speed and theta + * are at motor output, not gearbox output. */ +struct motor_t +{ + /* Motor and load characteristics. */ + struct motor_def_t m; /* Simulation parameters. */ double h; /* Simulation time step (s). */ int d; /* Simulation time step division. */ /* Simulation current state. */ - double t; /* Current time (not realy used) (s). */ + double t; /* Current time (not really used) (s). */ double u; /* Current input voltage (V). */ double i; /* Current current (A). */ double o; /* Current angular speed (o for omega) (rad/s). */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index a529a1ed..417ffea1 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -52,17 +52,18 @@ int16_t pwm_left, pwm_right, pwm_aux0; /** PWM reverse directions. */ uint8_t pwm_reverse; +/* Robot model. */ +const struct robot_t *simu_robot; + /** Motor models. */ struct motor_t simu_left_model, simu_right_model, simu_aux0_model; -/** Is there an auxiliary motor? */ -int simu_aux0; - -/** Computed simulated position (mm). */ +/** Computed simulated position (mm, rad). */ double simu_pos_x, simu_pos_y, simu_pos_a; -/** Distance between wheels. */ -double simu_footing; +/** Full counter values. */ +uint32_t simu_counter_left, simu_counter_right, simu_counter_aux0; +double simu_counter_left_th, simu_counter_right_th; /** Initialise simulation. */ static void @@ -70,34 +71,29 @@ simu_init (void) { int argc; char **argv; - const struct robot_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) + simu_robot = models_get (argv[0]); + if (!simu_robot) { fprintf (stderr, "unknown model name: %s\n", argv[0]); exit (1); } - simu_left_model = *m->motor; - simu_right_model = *m->motor; - simu_aux0 = !!m->aux0; - if (simu_aux0) - simu_aux0_model = *m->aux0; - simu_footing = m->footing; + models_init (simu_robot, &simu_left_model, &simu_right_model, + &simu_aux0_model); simu_pos_x = simu_pos_y = simu_pos_a = 0; } /** Update simulation position. */ static void -simu_pos_update (double dl, double dr) +simu_pos_update (double dl, double dr, double footing) { double d = 0.5 * (dl + dr); - double da = (dr - dl) / simu_footing; + double da = (dr - dl) / footing; double na = simu_pos_a + da; if (da < 0.0001 && da > -0.0001) { @@ -124,35 +120,71 @@ simu_step (void) assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX); assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX); assert (pwm_aux0 >= -PWM_MAX && pwm_aux0 <= PWM_MAX); - simu_left_model.u = (double) (pwm_left + 1) / (PWM_MAX + 1); - simu_right_model.u = (double) (pwm_right + 1) / (PWM_MAX + 1); - simu_aux0_model.u = (double) (pwm_aux0 + 1) / (PWM_MAX + 1); + simu_left_model.u = simu_left_model.m.u_max + * ((double) pwm_left / (PWM_MAX + 1)); + simu_right_model.u = simu_right_model.m.u_max + * ((double) pwm_right / (PWM_MAX + 1)); + simu_aux0_model.u = simu_aux0_model.m.u_max + * ((double) pwm_aux0 / (PWM_MAX + 1)); /* Make one step. */ old_left_th = simu_left_model.th; old_right_th = simu_right_model.th; old_aux0_th = simu_aux0_model.th; motor_model_step (&simu_left_model); motor_model_step (&simu_right_model); - if (simu_aux0) + if (simu_robot->aux0_motor) motor_model_step (&simu_aux0_model); /* Modify counters. */ - counter_left_diff = (simu_left_model.th - old_left_th) / (2*M_PI) - * 500 * simu_left_model.i_G; + uint32_t counter_left_new; + uint32_t counter_right_new; + if (!simu_robot->encoder_separated) + { + counter_left_new = simu_left_model.th / (2*M_PI) + * simu_robot->main_encoder_steps; + counter_right_new = simu_right_model.th / (2*M_PI) + * simu_robot->main_encoder_steps; + } + else + { + /* Thanks Thales. */ + double left_diff = (simu_left_model.th - old_left_th) + / simu_left_model.m.i_G * simu_robot->wheel_r; + double right_diff = (simu_right_model.th - old_right_th) + / simu_right_model.m.i_G * simu_robot->wheel_r; + double sum = left_diff + right_diff; + double diff = (left_diff - right_diff) + * (simu_robot->encoder_footing / simu_robot->footing); + double left_enc_diff = 0.5 * (sum + diff); + double right_enc_diff = 0.5 * (sum - diff); + simu_counter_left_th += left_enc_diff / simu_robot->encoder_wheel_r; + simu_counter_right_th += right_enc_diff / simu_robot->encoder_wheel_r; + counter_left_new = simu_counter_left_th / (2*M_PI) + * simu_robot->main_encoder_steps; + counter_right_new = simu_counter_right_th / (2*M_PI) + * simu_robot->main_encoder_steps; + } + /* Update an integer counter. */ + counter_left_diff = counter_left_new - simu_counter_left; counter_left += counter_left_diff; - counter_right_diff = (simu_right_model.th - old_right_th) / (2*M_PI) - * 500 * simu_right_model.i_G; + simu_counter_left = counter_left_new; + counter_right_diff = counter_right_new - simu_counter_right; counter_right += counter_right_diff; - if (simu_aux0) + simu_counter_right = counter_right_new; + /* Update auxiliary counter. */ + if (simu_robot->aux0_motor) { - counter_aux0_diff = (simu_aux0_model.th - old_aux0_th) / (2*M_PI) - * 500 * simu_aux0_model.i_G; + uint32_t counter_aux0_new = simu_aux0_model.th / (2*M_PI) + * simu_robot->aux0_encoder_steps; + counter_aux0_diff = counter_aux0_new - simu_counter_aux0; counter_aux0 += counter_aux0_diff; + simu_counter_aux0 = counter_aux0_new; } /* Update position */ simu_pos_update ((simu_left_model.th - old_left_th) - * simu_left_model.i_G * simu_left_model.w_r * 1000, + / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, (simu_right_model.th - old_right_th) - * simu_right_model.i_G * simu_right_model.w_r * 1000); + / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, + simu_robot->footing * 1000); } /** Initialise the timer. */ diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c index 8366d033..c5d4ea4a 100644 --- a/digital/asserv/src/asserv/test_motor_model.c +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -57,11 +57,11 @@ main (int argc, char **argv) fprintf (stderr, "model unknown\n"); return 1; } - ms = *mr->motor; + models_init (mr, &ms, NULL, NULL); m = &ms; /* Make a step response simulation. */ printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta"); - m->u = 3.0; + m->u = m->m.u_max; printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th); simu (m, 1.0); m->u = 0.0; -- cgit v1.2.3