From 969f9f57656573a8aacc99c2f810392629a67455 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 6 Apr 2012 22:13:09 +0200 Subject: digital/asserv: move robot specific functions to models.host.c --- digital/asserv/src/asserv/models.host.c | 168 ++++++++++++++++++++++++++++++-- 1 file changed, 161 insertions(+), 7 deletions(-) (limited to 'digital/asserv/src/asserv/models.host.c') diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index 72524932..d5c33eef 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -25,14 +25,38 @@ #define _GNU_SOURCE 1 /* Need ISO C99 features as well. */ #include "common.h" +#include "modules/utils/utils.h" + +#include "io.h" + #include "models.host.h" #include "simu.host.h" +#include "aux.h" + +#include AC_ASSERV_CONTACTS_H + #include #include #define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } } +static void +simu_sensor_update_giboulee (void); + +static void +simu_sensor_update_aquajim (void); + +static void +simu_sensor_update_marcel (void); + +static void +marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left, + struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[]); + +static int +simu_table_test_robospierre (double p_x, double p_y); + /* Gloubi, Efrei 2006. */ static const struct robot_t gloubi_robot = { @@ -185,13 +209,6 @@ static const struct robot_t aquajim_robot = NULL, NO_CORNER, NULL }; -void -marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left, - struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[]) -{ - aux_motor[0].m.th_min = 0.0; -} - /* Marcel, APBTeam 2010. */ static const struct robot_t marcel_robot = { @@ -336,3 +353,140 @@ models_init (const struct robot_t *robot, motor_model_t *main_motor_left, robot->init (robot, main_motor_left, main_motor_right, aux_motor); } +/** Update sensors for Giboulee. */ +void +simu_sensor_update_giboulee (void) +{ + /** Micro-switch sensors. */ + static const double sensors[][2] = + { + { -70.0, 200.0 }, + { -70.0, -200.0 }, + { 170.0, 0.0 }, + }; + static const uint8_t sensors_bit[] = + { IO_BV (CONTACT_BACK_LEFT_IO), IO_BV (CONTACT_BACK_RIGHT_IO), + IO_BV (CONTACT_CENTER_IO), }; + static const double table_width = 3000.0, table_height = 2100.0; + PINC = 0; + unsigned int i; + double x, y; + for (i = 0; i < UTILS_COUNT (sensors); i++) + { + /* Compute absolute position. */ + 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]; + } + /** Top zero sensor. */ + double aa; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + aa = simu_aux_model[i].th / simu_aux_model[i].m.i_G * 3; + if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5)) + *aux[i].zero_pin |= aux[i].zero_bv; + } +} + +/** Update sensors for AquaJim. */ +void +simu_sensor_update_aquajim (void) +{ + /** Micro-switch sensors. */ + static const double sensors[][2] = + { + { -150.0, 70.0 }, + { -150.0, -70.0 }, + { 150.0, 130.0 }, + { 150.0, -130.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. */ + 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; + 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 + /* Almost open. */ + + 2 * M_PI / 6 / 10 + /* Turn at the next hole. */ + - 2 * M_PI / 3 + /* Mechanical offset. */ + + 2 * M_PI * 0x43e / simu_robot->aux_encoder_steps[0] / + 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); +} + +/** 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. */ + 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]; + } + /** Top zero sensors. */ + if (simu_aux_model[1].th < 0) + PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); +} + +static void +marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left, + struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[]) +{ + aux_motor[0].m.th_min = 0.0; +} + +/* 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; +} + -- cgit v1.2.3