summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2012-03-21 00:46:03 +0100
committerNicolas Schodet2012-03-21 18:56:51 +0100
commit1e7c8a8a9811c45091e6c471482086113f059f40 (patch)
treed7faa80dd162799c394ec23925cc53b6dbff008e /digital/asserv
parent0feb542a8cf7630a5d442387727414f5251aad83 (diff)
parentdcb79f383269440ec2c5a54b6d7792fbb0110d5a (diff)
Merge branch 'master' into efrei-robotique
Conflicts: digital/mimot/src/dirty/aux.c digital/mimot/src/dirty/aux.h digital/mimot/src/dirty/contacts.h digital/mimot/src/dirty/main.c digital/mimot/src/dirty/models.host.c digital/mimot/src/dirty/twi_proto.c digital/mimot/tools/mimot/init.py
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/main.c21
-rw-r--r--digital/asserv/src/asserv/models.host.c65
-rw-r--r--digital/asserv/src/asserv/models.host.h7
-rw-r--r--digital/asserv/src/asserv/simu.host.c107
-rw-r--r--digital/asserv/src/asserv/simu.host.h3
-rw-r--r--digital/asserv/src/asserv/speed.c20
-rw-r--r--digital/asserv/src/asserv/test_motor_model.c6
-rw-r--r--digital/asserv/src/asserv/traj.c57
-rw-r--r--digital/asserv/src/asserv/traj.h4
-rw-r--r--digital/asserv/src/asserv/twi_proto.c18
-rw-r--r--digital/asserv/tools/asserv/asserv.py19
-rw-r--r--digital/asserv/tools/asserv/init.py26
-rw-r--r--digital/asserv/tools/inter_asserv.py2
-rw-r--r--digital/asserv/tools/test_goto.py33
14 files changed, 347 insertions, 41 deletions
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/models.host.c b/digital/asserv/src/asserv/models.host.c
index f4d8ca5a..3839af1e 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -32,6 +32,8 @@
#include <math.h>
#include <string.h>
+#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. */
+ -INFINITY, +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, 110 }, { 150, -110 }, { -150, -110 }, { -150, 110 } },
};
/* 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..7053ae60 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -36,6 +36,8 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
+#include <time.h>
+#include <sys/time.h>
#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");
@@ -154,6 +160,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 +191,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 +230,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 +278,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 +288,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 +325,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 +416,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 ();
@@ -450,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/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/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/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;
+}
+
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
@@ -49,6 +49,10 @@ 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);
void
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)))
diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py
index 3f0afa36..c845af03 100644
--- a/digital/asserv/tools/asserv/init.py
+++ b/digital/asserv/tools/asserv/init.py
@@ -12,10 +12,19 @@ host_marcel = dict (
E = 0x3ff, D = 0x1ff,
l = 0x1000,
)
+host_robospierre = dict (
+ scale = 0.0395840674352314, f = 0xdd1,
+ tkp = 1, tkd = 16,
+ ta = 0.75, tsm = 0x60, tss = 0x10,
+ akp = 2, akd = 16,
+ aa = 0.25, asm = 0x60, ass = 0x10,
+ E = 0x3ff, D = 0x1ff,
+ l = 0x1000,
+ )
host = {
'giboulee': host_marcel,
'marcel': host_marcel,
- 'robospierre': host_marcel,
+ 'robospierre': host_robospierre,
}
target_marcel = dict (
scale = 0.0415178942124, f = 0xcef,
@@ -34,8 +43,21 @@ target_marcel = dict (
l = 0x1000,
w = 0x09,
)
+target_robospierre = dict (
+ scale = 0.0317975134344, f = 0x134e,
+ c = float (0xffa897) / (1 << 24),
+ tkp = 1, tkd = 16,
+ ta = 0.75, tsm = 0x60, tss = 0x10,
+ tbe = 256, tbs = 0x08, tbc = 40,
+ akp = 2, akd = 16,
+ aa = 0.5, asm = 0x60, ass = 0x10,
+ abe = 128, abs = 0x08, abc = 40,
+ E = 0x3ff, D = 0x1ff,
+ l = 0x1000,
+ w = 0x00,
+ )
target = {
'giboulee': target_marcel,
'marcel': target_marcel,
- 'robospierre': target_marcel,
+ 'robospierre': target_robospierre,
}
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):
diff --git a/digital/asserv/tools/test_goto.py b/digital/asserv/tools/test_goto.py
index 2cbbb350..f2aa2535 100644
--- a/digital/asserv/tools/test_goto.py
+++ b/digital/asserv/tools/test_goto.py
@@ -3,14 +3,29 @@ import math
import asserv
import asserv.init
-from utils.init_proto import init_proto
+from utils.init_proto import InitProto
-a = init_proto (None, asserv.Proto, asserv.init)
-for i in xrange (10):
- x = random.randrange (2000)
- y = random.randrange (1100)
- a.goto (x, y)
- a.goto_angle (math.radians (random.randrange (360)))
-a.goto (0, 0)
-a.goto_angle (0)
+ip = InitProto (None, asserv.Proto, asserv.init)
+ip.parser.add_option ('-i', '--iterations',
+ help = "number of test iterations", metavar = 'NB', default = 10)
+ip.parser.add_option ('-t', '--type',
+ help = "test type, one of random or linear", metavar = 'TYPE',
+ default = 'random')
+ip.parse_args ()
+a = ip.get_proto ()
+try:
+ if ip.options.type == 'random':
+ for i in xrange (ip.options.iterations):
+ x = random.randrange (2000)
+ y = random.randrange (1100)
+ a.goto (x, y)
+ a.goto_angle (math.radians (random.randrange (360)))
+ a.goto (0, 0)
+ a.goto_angle (0)
+ elif ip.options.type == 'linear':
+ for i in xrange (ip.options.iterations):
+ a.speed_pos ('t', 1000)
+ a.speed_pos ('t', -1000)
+except:
+ pass
a.close ()