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/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 +++ 5 files changed, 136 insertions(+), 21 deletions(-) (limited to 'digital/asserv/src/asserv') 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 From 453c275fb70db0bdd7258c3bedbd8897fa0b35cc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 22:46:03 +0200 Subject: digital/asserv: slow down execution when running without mex --- digital/asserv/src/asserv/simu.host.c | 31 +++++++++++++++++++++++++++++++ digital/asserv/tools/inter_asserv.py | 2 +- 2 files changed, 32 insertions(+), 1 deletion(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index d9a97fe2..7053ae60 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -36,6 +36,8 @@ #include #include #include +#include +#include #include "pwm.h" #include "aux.h" @@ -87,6 +89,9 @@ double simu_counter_left_th, simu_counter_right_th; /** Use mex. */ int simu_mex; +/** Do not sleep. */ +int simu_fast; + /** Mex message types. */ uint8_t simu_mex_position; uint8_t simu_mex_pwm; @@ -115,6 +120,7 @@ simu_init (void) simu_mex_pwm = mex_node_reservef ("%s:pwm", mex_instance); simu_mex_aux = mex_node_reservef ("%s:aux", mex_instance); } + simu_fast = simu_mex; if (argc != 1) { fprintf (stderr, "Syntax: asserv.host [-m[interval]] model\n"); @@ -490,12 +496,37 @@ timer_init (void) simu_init (); } +/** Slow down program execution. */ +void +simu_wait (int freq) +{ +#define ONE_SEC_NS 1000000000ll + struct timeval tv; + static long long int last_ns = 0; + long long int now_ns; + gettimeofday (&tv, NULL); + now_ns = tv.tv_sec * ONE_SEC_NS + tv.tv_usec * 1000; + if (last_ns == 0 || now_ns - last_ns > ONE_SEC_NS) + last_ns = now_ns; + last_ns = last_ns + ONE_SEC_NS / freq; + long long int diff_ns = last_ns - now_ns; + if (diff_ns > 0) + { + struct timespec ts; + ts.tv_sec = diff_ns / ONE_SEC_NS; + ts.tv_nsec = diff_ns % ONE_SEC_NS; + nanosleep (&ts, &ts); + } +} + /** Wait for timer overflow. */ void timer_wait (void) { if (simu_mex) mex_node_wait_date (mex_node_date () + 4); + if (!simu_fast) + simu_wait (225); simu_step (); if (simu_mex && !--simu_send_cpt) { diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py index 82a92c32..72e31f60 100644 --- a/digital/asserv/tools/inter_asserv.py +++ b/digital/asserv/tools/inter_asserv.py @@ -43,7 +43,7 @@ class InterAsserv (Inter): self.tk.createfilehandler (self.a, READABLE, self.read) self.timeout () # Query position. - self.a.register_pos () + self.a.register_pos (interval = 225 / 20) self.a.position.register (self.pos) def createWidgets (self): -- cgit v1.2.3 From f753bc39682f980feb49b7ff1bb9e9594f986e6b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 23:24:08 +0200 Subject: digital/asserv: add push the wall traj mode, closes #167 --- digital/ai/src/twi_master/asserv.c | 22 ++++++++++++++ digital/ai/src/twi_master/asserv.h | 6 ++++ digital/asserv/src/asserv/main.c | 21 +++++++++++++ digital/asserv/src/asserv/traj.c | 57 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 4 +++ digital/asserv/src/asserv/twi_proto.c | 18 +++++++++++ digital/asserv/tools/asserv/asserv.py | 19 ++++++++++-- 7 files changed, 145 insertions(+), 2 deletions(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index d9ae9330..839b1d23 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -300,6 +300,28 @@ asserv_go_to_the_wall (uint8_t backward) twi_master_send_buffer (2); } +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a) +{ + if (init_x != (uint32_t) -1) + init_x = fixed_mul_f824 (init_x, asserv_scale_inv); + if (init_y != (uint32_t) -1) + init_y = fixed_mul_f824 (init_y, asserv_scale_inv); + uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); + buffer[0] = 'G'; + buffer[1] = backward; + buffer[2] = v32_to_v8 (init_x, 2); + buffer[3] = v32_to_v8 (init_x, 1); + buffer[4] = v32_to_v8 (init_x, 0); + buffer[5] = v32_to_v8 (init_y, 2); + buffer[6] = v32_to_v8 (init_y, 1); + buffer[7] = v32_to_v8 (init_y, 0); + buffer[8] = v16_to_v8 (init_a, 1); + buffer[9] = v16_to_v8 (init_a, 0); + twi_master_send_buffer (10); +} + void asserv_move_motor0_absolute (uint16_t position, uint8_t speed) { diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 2aa7a6f8..c9a52a11 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -198,6 +198,12 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward); void asserv_go_to_the_wall (uint8_t backward); +/** Push the wall and initialise position. Use -1 for coordinates to keep + * unchanged. */ +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a); + /** * Move the motor0. * Motor0 class command. diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 2d53e2fa..677fea84 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -400,6 +400,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_ftw_start_center (args[0], args[1], args[2]); break; + case c ('f', 12): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - d: init_x, f24.8. + * - d: init_y, f24.8. + * - w: init_a, f0.16. + * - b: sequence number. */ + { + if (args[11] == state_main.sequence) + break; + int32_t angle; + if (args[9] == 0xff && args[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, args[9], args[10], 0); + traj_ptw_start (args[0], + v8_to_v32 (args[1], args[2], args[3], args[4]), + v8_to_v32 (args[5], args[6], args[7], args[8]), + angle, args[11]); + } + break; case c ('F', 1): /* Go to the dispenser. * - b: sequence number. */ diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 11618eca..e147ad5d 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -37,6 +37,7 @@ #include "pos.h" #include "speed.h" #include "postrack.h" +#include "pwm.h" #include "contacts.h" @@ -59,6 +60,8 @@ enum TRAJ_FTW, /* Go to the dispenser. */ TRAJ_GTD, + /* Push the wall. */ + TRAJ_PTW, /* Go to position. */ TRAJ_GOTO, /* Go to angle. */ @@ -99,6 +102,9 @@ static uint8_t traj_use_center; /** Center sensor delay. */ static uint8_t traj_center_delay; +/** Initial values for x, y and angle, or -1. */ +static int32_t traj_init_x, traj_init_y, traj_init_a; + /** Initialise computed factors. */ void traj_init (void) @@ -210,6 +216,54 @@ traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq) state_start (&state_main, MODE_TRAJ, seq); } +/** Push the wall mode. */ +static void +traj_ptw (void) +{ + /* If blocking, the wall was found. */ + if (pos_theta.blocked_counter >= pos_theta.blocked_counter_limit) + { + /* Initialise position. */ + if (traj_init_x != -1) + postrack_x = traj_init_x; + if (traj_init_y != -1) + postrack_y = traj_init_y; + if (traj_init_a != -1) + postrack_a = traj_init_a; + /* Stop motor control. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.variant = 0; + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start push the wall mode. Position is initialised unless -1. */ +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq) +{ + int16_t speed; + traj_mode = TRAJ_PTW; + traj_init_x = init_x; + traj_init_y = init_y; + traj_init_a = init_a; + state_start (&state_main, MODE_TRAJ, seq); + /* Use slow speed, without alpha control. */ + speed = speed_theta.slow; + speed *= 256; + if (backward) + speed = -speed; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = speed; + speed_alpha.cons = 0; + state_main.variant = 2; +} + /** Go to the dispenser mode. */ static void traj_gtd (void) @@ -392,6 +446,9 @@ traj_update (void) case TRAJ_FTW: traj_ftw (); break; + case TRAJ_PTW: + traj_ptw (); + break; case TRAJ_GTD: traj_gtd (); break; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 706fbdaf..ec345d0c 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -48,6 +48,10 @@ traj_ftw_start (uint8_t backward, uint8_t seq); void traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq); +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq); + void traj_gtd_start (uint8_t seq); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index ff15c82f..fcf0a9cc 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -168,6 +168,24 @@ twi_proto_callback (u8 *buf, u8 size) * - b: 0: forward, 1: backward. */ traj_ftw_start (buf[2], 0); break; + case c ('G', 9): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - 3b: init_x. + * - 3b: init_y. + * - w: init_a. */ + { + int32_t angle; + if (buf[9] == 0xff && buf[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, buf[9], buf[10], 0); + traj_ptw_start (buf[2], + v8_to_v32 (buf[3], buf[4], buf[5], 0xff), + v8_to_v32 (buf[6], buf[7], buf[8], 0xff), + angle, 0); + } + break; case c ('g', 2): /* Go to the wall using center sensor with delay. * - b: 0: forward, 1: backward. diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 997b05fb..4f386d7a 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -269,6 +269,15 @@ class Proto: self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) self.wait (self.finished, auto = True) + def ptw (self, backward = True, init_x = None, init_y = None, + init_a = None): + """Push the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BllHB', backward and 1 or 0, + self._dist_f248 (init_x), self._dist_f248 (init_y), + self._angle_f16 (init_a), self.mseq) + self.wait (self.finished, auto = True) + def set_simu_pos (self, x, y, a): """Set simulated position.""" self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), @@ -381,10 +390,16 @@ class Proto: return int (round (d / self.param['scale'])) def _dist_f248 (self, d): - return int (round ((1 << 8) * d / self.param['scale'])) + if d is None: + return -1 + else: + return int (round ((1 << 8) * d / self.param['scale'])) def _angle_f16 (self, a): - return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff + if a is None: + return 0xffff + else: + return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff def _angle_f824 (self, a): return int (round ((1 << 24) * a / (2 * math.pi))) -- cgit v1.2.3 From c3274377f05073c6d40265ab7849df2a72cfd54e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 20 May 2011 08:02:58 +0200 Subject: digital/asserv: fix bad model description --- digital/asserv/src/asserv/models.host.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index ae123585..3839af1e 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -107,7 +107,7 @@ static const struct motor_def_t faulhaber_2657_model = /* Load characteristics. */ 0.0, /* Load (kg.m^2). */ /* Hardware limits. */ - 0.0, +INFINITY, + -INFINITY, +INFINITY, }; /* Gloubi, Efrei 2006. */ @@ -395,7 +395,7 @@ static const struct robot_t robospierre_robot = * table. */ simu_table_test_robospierre, /** Robot corners, from front left, then clockwise. */ - { { 150, 50 }, { 150, -50 }, { -150, -50 }, { -150, 50 } }, + { { 150, 110 }, { 150, -110 }, { -150, -110 }, { -150, 110 } }, }; /* Table of models. */ -- cgit v1.2.3 From c3fe438f1b60f20f6f5425db895777ee5248637f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 11 Feb 2012 16:22:57 +0100 Subject: digital/mimot, digital/asserv: fix overflow in speed control code When substracting two values, one extra bit is needed. Use an unsigned number to have this extra bit. --- digital/asserv/src/asserv/speed.c | 20 ++++++++++++++------ digital/mimot/src/dirty/speed.c | 20 ++++++++++++++------ 2 files changed, 28 insertions(+), 12 deletions(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index 5c221b7e..32102177 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -58,13 +58,21 @@ speed_init (void) static void speed_update_by_speed (struct speed_t *speed) { - /* Update current speed. */ - if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) - speed->cur = speed->cons; - else if (speed->cons > speed->cur) - speed->cur += speed->acc; + /* Update current speed (be careful of overflow!). */ + if (speed->cons > speed->cur) + { + if ((uint16_t) (speed->cons - speed->cur) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur += speed->acc; + } else - speed->cur -= speed->acc; + { + if ((uint16_t) (speed->cur - speed->cons) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur -= speed->acc; + } } /** Compute maximum allowed speed according to: distance left, maximum speed, diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c index f9d4193e..80ca3d6b 100644 --- a/digital/mimot/src/dirty/speed.c +++ b/digital/mimot/src/dirty/speed.c @@ -53,13 +53,21 @@ speed_init (void) static void speed_update_by_speed (struct speed_t *speed) { - /* Update current speed. */ - if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) - speed->cur = speed->cons; - else if (speed->cons > speed->cur) - speed->cur += speed->acc; + /* Update current speed (be careful of overflow!). */ + if (speed->cons > speed->cur) + { + if ((uint16_t) (speed->cons - speed->cur) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur += speed->acc; + } else - speed->cur -= speed->acc; + { + if ((uint16_t) (speed->cur - speed->cons) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur -= speed->acc; + } } /** Compute maximum allowed speed according to: distance left, maximum speed, -- cgit v1.2.3