From 9d26b60d08f937ba49962ebfeaa3664f5527bf1a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 17 May 2011 01:58:19 +0200 Subject: digital/asserv: add robospierre model --- digital/ai/tools/robospierre.py | 3 +- digital/asserv/src/asserv/models.host.c | 65 ++++++++++++++++++++++-- digital/asserv/src/asserv/models.host.h | 7 +++ digital/asserv/src/asserv/simu.host.c | 76 +++++++++++++++++++++------- digital/asserv/src/asserv/simu.host.h | 3 ++ digital/asserv/src/asserv/test_motor_model.c | 6 +++ 6 files changed, 138 insertions(+), 22 deletions(-) diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 09be4bea..8d5db276 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -23,7 +23,8 @@ class Robot: self.robot_link = simu.robots.robospierre.link.bag self.robot_model = simu.robots.robospierre.model.bag self.robot_view = simu.robots.robospierre.view.bag - asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel') + asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', + 'robospierre') mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index f4d8ca5a..ae123585 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -32,6 +32,8 @@ #include #include +#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } } + /* RE25CLL with 1:10 gearbox model. */ static const struct motor_def_t re25cll_model = { @@ -89,6 +91,25 @@ static const struct motor_def_t amax32ghp_model = -INFINITY, +INFINITY, }; +/* Faulhaber 2657 and a 1:9.7 ratio gearbox. */ +static const struct motor_def_t faulhaber_2657_model = +{ + /* Motor characteristics. */ + 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 34.8 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.84, /* Terminal resistance (Ohm). */ + 0.380 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 9.7, /* Gearbox ratio. */ + 0.80, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + 0.0, +INFINITY, +}; + /* Gloubi, Efrei 2006. */ static const struct robot_t gloubi_robot = { @@ -106,7 +127,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 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* Taz, APBTeam/Efrei 2005. */ @@ -126,7 +147,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 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* TazG, Taz with RE25G motors. */ @@ -146,7 +167,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 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */ @@ -197,6 +218,7 @@ static const struct robot_t giboulee_robot = { 500, 0 }, /** Sensor update function. */ simu_sensor_update_giboulee, + NULL, NO_CORNER, }; /* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */ @@ -267,6 +289,7 @@ static const struct robot_t aquajim_robot = { 250, 250 }, /** Sensor update function. */ simu_sensor_update_aquajim, + NULL, NO_CORNER, }; /* Marcel elevator model, with a Faulhaber 2657 and a 1:9.7 ratio gearbox. */ @@ -338,6 +361,41 @@ static const struct robot_t marcel_robot = { 256, 250 }, /** Sensor update function. */ simu_sensor_update_marcel, + NULL, NO_CORNER, +}; + +/* Robospierre, APBTeam 2011. */ +static const struct robot_t robospierre_robot = +{ + /* Main motors. */ + &faulhaber_2657_model, + /* Number of steps on the main motors encoders. */ + 2500, + /* 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.0, + /* 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, + /** Auxiliary motors, NULL if not present. */ + { NULL, NULL }, + /** Number of steps for each auxiliary motor encoder. */ + { 0, 0 }, + /** Sensor update function. */ + NULL, + /** Table test function, return false if given robot point is not in + * table. */ + simu_table_test_robospierre, + /** Robot corners, from front left, then clockwise. */ + { { 150, 50 }, { 150, -50 }, { -150, -50 }, { -150, 50 } }, }; /* Table of models. */ @@ -352,6 +410,7 @@ static const struct { "giboulee", &giboulee_robot }, { "aquajim", &aquajim_robot }, { "marcel", &marcel_robot }, + { "robospierre", &robospierre_robot }, { 0, 0 } }; diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index 403e9cd9..d8f1dcb8 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -27,6 +27,8 @@ #define ECHANT_PERIOD (1.0 / (14745600.0 / 256 / 256)) +#define CORNERS_NB 4 + /** Define a robot and its peripherals. * Encoder characteristics are defined at gearbox output. */ struct robot_t @@ -55,6 +57,11 @@ struct robot_t int aux_encoder_steps[AC_ASSERV_AUX_NB]; /** Sensor update function. */ void (*sensor_update) (void); + /** Table test function, return false if given robot point is not in + * table. */ + int (*table_test) (double p_x, double p_y); + /** Robot corners, from front left, then clockwise. */ + double corners[CORNERS_NB][2]; }; /** 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 10cef711..d9a97fe2 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -154,6 +154,16 @@ simu_pos_update (double dl, double dr, double footing) simu_pos_a = na; } +/** Compute a robot point absolute position. */ +static void +simu_compute_absolute_position (double p_x, double p_y, double *x, double *y) +{ + double c = cos (simu_pos_a); + double s = sin (simu_pos_a); + *x = simu_pos_x + c * p_x - s * p_y; + *y = simu_pos_y + s * p_x + c * p_y; +} + /** Update sensors for Giboulee. */ void simu_sensor_update_giboulee (void) @@ -175,10 +185,7 @@ simu_sensor_update_giboulee (void) 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]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height) PINC |= sensors_bit[i]; } @@ -217,10 +224,7 @@ simu_sensor_update_aquajim (void) 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]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); cx = table_width / 2 - x; cy = table_height / 2 - y; ds = cx * cx + cy * cy; @@ -268,10 +272,7 @@ simu_sensor_update_marcel (void) 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]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height && (x < stand_x_min || x >= stand_x_max || y < stand_y)) PINC |= sensors_bit[i]; @@ -281,6 +282,18 @@ simu_sensor_update_marcel (void) PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); } +/* Table test for Robospierre. */ +int +simu_table_test_robospierre (double p_x, double p_y) +{ + static const double table_width = 3000.0, table_height = 2100.0; + double x, y; + simu_compute_absolute_position (p_x, p_y, &x, &y); + if (x < 0 || y < 0 || x >= table_width || y >= table_height) + return 0; + return 1; +} + /** Do a simulation step. */ static void simu_step (void) @@ -306,6 +319,39 @@ simu_step (void) if (simu_robot->aux_motor[i]) motor_model_step (&simu_aux_model[i]); } + /* Update position. */ + double old_pos_x = simu_pos_x, old_pos_y = simu_pos_y, + old_pos_a = simu_pos_a; + simu_pos_update ((simu_left_model.th - old_left_th) + / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, + (simu_right_model.th - old_right_th) + / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, + simu_robot->footing * 1000); + /* Check robot is still on the table. */ + if (simu_robot->table_test) + { + static int old_out = 1; + int out = 0; + for (i = 0; i < CORNERS_NB; i++) + { + if (!simu_robot->table_test (simu_robot->corners[i][0], + simu_robot->corners[i][1])) + out = 1; + } + /* If out, cancel movement. */ + if (out && !old_out) + { + simu_pos_x = old_pos_x; + simu_pos_y = old_pos_y; + simu_pos_a = old_pos_a; + simu_left_model.th = old_left_th; + simu_right_model.th = old_right_th; + } + else + { + old_out = out; + } + } /* Modify counters. */ uint32_t counter_left_new; uint32_t counter_right_new; @@ -364,12 +410,6 @@ simu_step (void) simu_counter_aux[i] = 0; } } - /* Update position. */ - simu_pos_update ((simu_left_model.th - old_left_th) - / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, - (simu_right_model.th - old_right_th) - / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, - simu_robot->footing * 1000); /* Update sensors. */ if (simu_robot->sensor_update) simu_robot->sensor_update (); diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index 143d3430..93785186 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -47,4 +47,7 @@ simu_sensor_update_aquajim (void); void simu_sensor_update_marcel (void); +int +simu_table_test_robospierre (double p_x, double p_y); + #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 706aa31b..044bab2e 100644 --- a/digital/asserv/src/asserv/test_motor_model.c +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -84,3 +84,9 @@ simu_sensor_update_marcel (void) { } +int +simu_table_test_robospierre (double p_x, double p_y) +{ + return 0; +} + -- cgit v1.2.3