From a4b92cbfb7aefbe35776967d0e249538e9a6d548 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 1 Apr 2010 23:57:04 +0200 Subject: digital/asserv: add 2010 robot model, closes #127 --- digital/asserv/src/asserv/models.host.c | 30 ++++++++++++++++++++++++ digital/asserv/src/asserv/simu.host.c | 34 ++++++++++++++++++++++++++++ digital/asserv/src/asserv/simu.host.h | 3 +++ digital/asserv/src/asserv/test_motor_model.c | 5 ++++ 4 files changed, 72 insertions(+) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index 75a68b1d..08ae3804 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -256,6 +256,35 @@ static const struct robot_t aquajim_robot = simu_sensor_update_aquajim, }; +/* Marcel, APBTeam 2010. */ +static const struct robot_t marcel_robot = +{ + /* Main motors. */ + &amax32ghp_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. */ + simu_sensor_update_marcel, +}; + /* Table of models. */ static const struct { @@ -267,6 +296,7 @@ static const struct { "tazg", &tazg_robot }, { "giboulee", &giboulee_robot }, { "aquajim", &aquajim_robot }, + { "marcel", &marcel_robot }, { 0, 0 } }; diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 07e441ea..15bb1ef9 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -234,6 +234,40 @@ simu_sensor_update_aquajim (void) PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); } +/** Update sensors for Marcel. */ +void +simu_sensor_update_marcel (void) +{ + /** Micro-switch sensors. */ + static const double sensors[][2] = + { + { -160.0, 90.0 }, + { -160.0, -90.0 }, + { 120.0, 155.0 }, + { 120.0, -155.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), }; + static const double table_width = 3000.0, table_height = 2100.0; + static const double stand_x_min = 1500.0 - 759.5, + stand_x_max = 1500.0 + 759.5, stand_y = 2100.0 - 522.0; + PINC = 0; + unsigned int i; + double x, y; + 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]; + 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]; + } +} + /** Do a simulation step. */ static void simu_step (void) diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index b3b5b0df..143d3430 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -44,4 +44,7 @@ simu_sensor_update_giboulee (void); void simu_sensor_update_aquajim (void); +void +simu_sensor_update_marcel (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 c6e60c81..706aa31b 100644 --- a/digital/asserv/src/asserv/test_motor_model.c +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -79,3 +79,8 @@ simu_sensor_update_aquajim (void) { } +void +simu_sensor_update_marcel (void) +{ +} + -- cgit v1.2.3