From 6b3d5061623a35c96e66e25fc14af6a9323f81b0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 8 May 2009 22:48:37 +0200 Subject: * digital/asserv: - added AquaJim simulated contacts. --- digital/asserv/src/asserv/models.host.c | 13 +++++-- digital/asserv/src/asserv/models.host.h | 2 ++ digital/asserv/src/asserv/simu.host.c | 54 +++++++++++++++++++++++++--- digital/asserv/src/asserv/simu.host.h | 6 ++++ digital/asserv/src/asserv/test_motor_model.c | 11 ++++++ digital/asserv/src/asserv/traj.c | 2 +- 6 files changed, 80 insertions(+), 8 deletions(-) diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index 95446784..75a68b1d 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -22,8 +22,11 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" + #include "motor_model.host.h" #include "models.host.h" +#include "simu.host.h" #include #include @@ -96,7 +99,7 @@ static const struct robot_t gloubi_robot = 13.0, // approx /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 } + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL }; /* Taz, APBTeam/Efrei 2005. */ @@ -116,7 +119,7 @@ static const struct robot_t taz_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 } + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL }; /* TazG, Taz with RE25G motors. */ @@ -136,7 +139,7 @@ static const struct robot_t tazg_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 } + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL }; /* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */ @@ -183,6 +186,8 @@ static const struct robot_t giboulee_robot = { &giboulee_arm_model, NULL }, /** Number of steps for each auxiliary motor encoder. */ { 500, 0 }, + /** Sensor update function. */ + simu_sensor_update_giboulee, }; /* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */ @@ -247,6 +252,8 @@ static const struct robot_t aquajim_robot = { &aquajim_arm_model, &aquajim_elevator_model }, /** Number of steps for each auxiliary motor encoder. */ { 250, 250 }, + /** Sensor update function. */ + simu_sensor_update_aquajim, }; /* Table of models. */ diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index ae2bc21d..403e9cd9 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -53,6 +53,8 @@ struct robot_t const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB]; /** Number of steps for each auxiliary motor encoder. */ int aux_encoder_steps[AC_ASSERV_AUX_NB]; + /** Sensor update function. */ + void (*sensor_update) (void); }; /** Get a pointer to a model by name, or return 0. */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index f89fabd4..321c9f9d 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -139,9 +139,9 @@ simu_pos_update (double dl, double dr, double footing) simu_pos_a = na; } -/** Update sensors. */ -static void -simu_sensor_update (void) +/** Update sensors for Giboulee. */ +void +simu_sensor_update_giboulee (void) { /** Micro-switch sensors. */ static const double sensors[][2] = @@ -177,6 +177,51 @@ simu_sensor_update (void) } } +/** Update sensors for AquaJim. */ +void +simu_sensor_update_aquajim (void) +{ + /** Micro-switch sensors. */ + static const double sensors[][2] = + { + { -150.0, 150.0 }, + { -150.0, -150.0 }, + { 150.0, 150.0 }, + { 150.0, -150.0 }, + { 150.0, 0.0 }, + }; + static const uint8_t sensors_bit[] = + { IO_BV (CONTACT_BACK_LEFT_IO), IO_BV (CONTACT_BACK_RIGHT_IO), + IO_BV (CONTACT_FRONT_LEFT_IO), IO_BV (CONTACT_FRONT_RIGHT_IO), + IO_BV (CONTACT_CENTER_IO), }; + static const double table_width = 3000.0, table_height = 2100.0; + static const double center_zone_radius = 150.0; + PINC = 0; + unsigned int i; + double x, y, cx, cy, ds; + for (i = 0; i < UTILS_COUNT (sensors); i++) + { + /* Compute absolute position. */ + x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] + - sin (simu_pos_a) * sensors[i][1]; + y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] + + cos (simu_pos_a) * sensors[i][1]; + cx = table_width / 2 - x; + cy = table_height / 2 - y; + ds = cx * cx + cy * cy; + if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height + && ds > center_zone_radius * center_zone_radius) + PINC |= sensors_bit[i]; + } + /** Top zero sensors. */ + double aa = simu_aux_model[0].th / simu_aux_model[0].m.i_G; + double apos = aa / (2 * M_PI / 3); + if (apos - floor (apos) > 0.5) + PINC |= IO_BV (CONTACT_AUX0_ZERO_IO); + if (simu_aux_model[1].th >= 0) + PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); +} + /** Do a simulation step. */ static void simu_step (void) @@ -267,7 +312,8 @@ simu_step (void) / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, simu_robot->footing * 1000); /* Update sensors. */ - simu_sensor_update (); + if (simu_robot->sensor_update) + simu_robot->sensor_update (); } /** Send information to the other nodes. */ diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index e5722369..b3b5b0df 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -38,4 +38,10 @@ timer_wait (void); uint8_t timer_read (void); +void +simu_sensor_update_giboulee (void); + +void +simu_sensor_update_aquajim (void); + #endif /* simu_host_h */ diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c index c5d4ea4a..c6e60c81 100644 --- a/digital/asserv/src/asserv/test_motor_model.c +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -68,3 +68,14 @@ main (int argc, char **argv) simu (m, 1.0); return 0; } + +void +simu_sensor_update_giboulee (void) +{ +} + +void +simu_sensor_update_aquajim (void) +{ +} + diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 6e7c2bc7..92ff94f1 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -151,7 +151,7 @@ traj_ftw (void) state_main.variant = 2; #else /* On host, we must do the job. */ - speed_theta.cons = speed / 2; + speed_theta.cons = speed / 4; if (left) speed_alpha.cons = speed / 2; else -- cgit v1.2.3