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 ++++++++++++++++++++++++++++++-- digital/asserv/src/asserv/simu.host.c | 140 +------------------------- digital/asserv/src/asserv/simu.host.h | 19 ++-- 3 files changed, 171 insertions(+), 156 deletions(-) (limited to 'digital') 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; +} + diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 04ade3aa..b222b305 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -27,11 +27,8 @@ #include "modules/host/host.h" #include "modules/host/mex.h" -#include "modules/utils/utils.h" #include "modules/math/fixed/fixed.h" -#include "io.h" - #include #include #include @@ -40,14 +37,11 @@ #include #include "cs.h" -#include "aux.h" - -#include AC_ASSERV_CONTACTS_H #include "models.host.h" /** Simulate some AVR regs. */ -uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; +uint8_t DDRF, PORTB, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; /** Index of loaded eeprom block. */ int8_t eeprom_loaded = -1; @@ -142,7 +136,7 @@ simu_pos_update (double dl, double dr, double footing) } /** Compute a robot point absolute position. */ -static void +void simu_compute_absolute_position (double p_x, double p_y, double *x, double *y) { double c = cos (simu_pos_a); @@ -151,136 +145,6 @@ simu_compute_absolute_position (double p_x, double p_y, double *x, double *y) *y = simu_pos_y + s * p_x + c * p_y; } -/** 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); -} - -/* 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) diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index 93785186..2d30ce97 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -24,11 +24,17 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "models.host.h" -extern uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; +extern uint8_t DDRF, PORTB, PORTC, PORTD, PORTE, PORTF, PORTG, PINC; extern double simu_pos_x, simu_pos_y, simu_pos_a; +extern motor_model_t simu_left_model, simu_right_model, + simu_aux_model[AC_ASSERV_AUX_NB]; + +extern const struct robot_t *simu_robot; + void timer_init (void); @@ -39,15 +45,6 @@ uint8_t timer_read (void); void -simu_sensor_update_giboulee (void); - -void -simu_sensor_update_aquajim (void); - -void -simu_sensor_update_marcel (void); - -int -simu_table_test_robospierre (double p_x, double p_y); +simu_compute_absolute_position (double p_x, double p_y, double *x, double *y); #endif /* simu_host_h */ -- cgit v1.2.3