From aa6c1c3b45aa4f834b498adddfa96adf6f0ed6c6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 11 Mar 2008 00:47:35 +0100 Subject: * digital/asserv/src/asserv: - added auxiliary motor to counter and simu. - simu needs some rewrite as counters are no longer on the motor axis. --- digital/asserv/src/asserv/avrconfig.h | 2 +- digital/asserv/src/asserv/main.c | 2 +- digital/asserv/src/asserv/models.host.c | 38 +++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/models.host.h | 1 + digital/asserv/src/asserv/simu.host.c | 30 ++++++++++++++++++++------ 5 files changed, 65 insertions(+), 8 deletions(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h index ef75a877..f1177995 100644 --- a/digital/asserv/src/asserv/avrconfig.h +++ b/digital/asserv/src/asserv/avrconfig.h @@ -59,7 +59,7 @@ /** In HOST compilation: * - STDIO: use stdin/out. * - PTS: use pseudo terminal. */ -#define AC_UART0_HOST_DRIVER PTS +#define AC_UART0_HOST_DRIVER STDIO /** Same thing for secondary port. */ #define AC_UART1_PORT -1 #define AC_UART1_BAUDRATE 115200 diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index e8f43c4a..93938226 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -148,7 +148,7 @@ main_loop (void) } if (main_stat_counter && !--main_stat_counter_cpt) { - proto_send2w ('C', counter_left, counter_right); + proto_send3w ('C', counter_left, counter_right, counter_aux0); main_stat_counter_cpt = main_stat_counter; } if (main_stat_postrack && !--main_stat_postrack_cpt) diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index e98631f7..7436ed43 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -58,6 +58,7 @@ static const struct motor_t gloubi_model = static const struct robot_t gloubi_robot = { &gloubi_model, + NULL, 26.0, /* Distance between the wheels (m). */ }; @@ -91,6 +92,7 @@ static const struct motor_t taz_model = static const struct robot_t taz_robot = { &taz_model, + NULL, 30.0, /* Distance between the wheels (m). */ }; @@ -124,6 +126,41 @@ static const struct motor_t tazg_model = static const struct robot_t tazg_robot = { &tazg_model, + NULL, + 30.0, /* Distance between the wheels (m). */ +}; + +/* Gerard arm model, with a RE25CLL and a 1:10 ratio gearbox. */ +static const struct motor_t gerard_arm_model = +{ + /* Motor caracteristics. */ + 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. */ + 10, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load caracteristics. */ + 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). */ +}; + +static const struct robot_t gerard_robot = +{ + &tazg_model, + &gerard_arm_model, 30.0, /* Distance between the wheels (m). */ }; @@ -136,6 +173,7 @@ static const struct { "gloubi", &gloubi_robot }, { "taz", &taz_robot }, { "tazg", &tazg_robot }, + { "gerard", &gerard_robot }, { 0, 0 } }; diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index bd459a1c..a63a2776 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -28,6 +28,7 @@ struct robot_t { const struct motor_t *motor; + const struct motor_t *aux0; double footing; /* Distance between the wheels (m). */ }; diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index ca5f18dc..a529a1ed 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -41,10 +41,10 @@ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; /** Overall counter values. */ -uint16_t counter_left, counter_right; +uint16_t counter_left, counter_right, counter_aux0; /** Counter differences since last update. - * Maximum of 9 significant bits, sign included. */ -int16_t counter_left_diff, counter_right_diff; + * Maximum of 7 significant bits, sign included. */ +int16_t counter_left_diff, counter_right_diff, counter_aux0_diff; /** PWM values, this is an error if absolute value is greater than the * maximum. */ @@ -52,7 +52,11 @@ int16_t pwm_left, pwm_right, pwm_aux0; /** PWM reverse directions. */ uint8_t pwm_reverse; -struct motor_t simu_left_model, simu_right_model; +/** 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). */ double simu_pos_x, simu_pos_y, simu_pos_a; @@ -81,8 +85,11 @@ simu_init (void) } 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; - simu_pos_x = simu_pos_y = 0; + simu_pos_x = simu_pos_y = simu_pos_a = 0; } /** Update simulation position. */ @@ -112,17 +119,22 @@ simu_pos_update (double dl, double dr) static void simu_step (void) { - double old_left_th, old_right_th; + double old_left_th, old_right_th, old_aux0_th; /* Convert pwm value into voltage. */ 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); /* 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) + 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; @@ -130,6 +142,12 @@ simu_step (void) counter_right_diff = (simu_right_model.th - old_right_th) / (2*M_PI) * 500 * simu_right_model.i_G; counter_right += counter_right_diff; + if (simu_aux0) + { + counter_aux0_diff = (simu_aux0_model.th - old_aux0_th) / (2*M_PI) + * 500 * simu_aux0_model.i_G; + counter_aux0 += counter_aux0_diff; + } /* Update position */ simu_pos_update ((simu_left_model.th - old_left_th) * simu_left_model.i_G * simu_left_model.w_r * 1000, -- cgit v1.2.3