summaryrefslogtreecommitdiff
path: root/digital/ai
diff options
context:
space:
mode:
Diffstat (limited to 'digital/ai')
-rw-r--r--digital/ai/src/common/defs.h11
-rw-r--r--digital/ai/src/fsm/fsm.host.c56
-rw-r--r--digital/ai/src/fsm/init.c185
-rw-r--r--digital/ai/src/move/radar.c168
-rw-r--r--digital/ai/src/move/radar.h96
-rw-r--r--digital/ai/src/twi_master/asserv.c28
-rw-r--r--digital/ai/src/twi_master/asserv.h14
-rw-r--r--digital/ai/src/twi_master/mimot.c82
-rw-r--r--digital/ai/src/twi_master/mimot.h30
-rw-r--r--digital/ai/src/utils/chrono.c7
-rw-r--r--digital/ai/src/utils/chrono.h8
-rw-r--r--digital/ai/tools/marcel.py58
-rw-r--r--digital/ai/tools/robospierre.py60
-rw-r--r--digital/ai/tools/test_simu.py81
-rw-r--r--digital/ai/tools/test_simu_control_marcel.py7
-rw-r--r--digital/ai/tools/test_simu_control_robospierre.py63
16 files changed, 815 insertions, 139 deletions
diff --git a/digital/ai/src/common/defs.h b/digital/ai/src/common/defs.h
index f16e9e52..12c47584 100644
--- a/digital/ai/src/common/defs.h
+++ b/digital/ai/src/common/defs.h
@@ -43,11 +43,18 @@ typedef struct position_t position_t;
/** Convert degrees to an angle usable in position_t. */
#define POSITION_A_DEG(a) G_ANGLE_UF016_DEG (a)
+/** No particular direction. */
+#define DIRECTION_NONE 0
+/** Forward direction, along the robot X axis. */
+#define DIRECTION_FORWARD 1
+/** Backward, opposite the robot X axis. */
+#define DIRECTION_BACKWARD 2
+
/** Team color, determine the start zone side. */
enum team_color_e
{
- TEAM_COLOR_LEFT = 0,
- TEAM_COLOR_RIGHT = 1
+ TEAM_COLOR_RIGHT = 0,
+ TEAM_COLOR_LEFT = 1
};
/** Current team color, to be read at start up. */
diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c
index bf8b761d..50336722 100644
--- a/digital/ai/src/fsm/fsm.host.c
+++ b/digital/ai/src/fsm/fsm.host.c
@@ -1056,11 +1056,18 @@ fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings)
/* Gen function table. */
fprintf (f, "typedef fsm_%s_branch_t (*fsm_%s_func_t)(void);\n", fsm->name,
fsm->name);
- fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n\n",
+ fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n",
fsm->name,
fsm->name,
fsm->event_nb,
fsm->state_nb);
+ /* Gen read function for trans table. */
+ fprintf (f, "fsm_%s_func_t fsm_%s_read_trans (fsm_%s_event_t event, "
+ "fsm_%s_state_t state);\n\n",
+ fsm->name,
+ fsm->name,
+ fsm->name,
+ fsm->name);
/* Gen active states array. */
fprintf (f, "extern fsm_%s_state_t fsm_%s_active_states[%u];\n\n",
@@ -1137,6 +1144,7 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
/* Introduction. */
fprintf (f, "/* This file has been generated, do not edit. */\n\n");
fprintf (f, "#include \"fsm_%s_gen.h\"\n\n", fsm->name);
+ fprintf (f, "#include \"modules/proto/proto.h\"\n\n");
/* Gen state strings. */
if (embedded_strings)
@@ -1280,6 +1288,19 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
}
fprintf (f, "};\n\n");
+ /* Gen read function for trans table. */
+ fprintf (f, "fsm_%s_func_t fsm_%s_read_trans (fsm_%s_event_t event, "
+ "fsm_%s_state_t state)\n{\n",
+ fsm->name,
+ fsm->name,
+ fsm->name,
+ fsm->name);
+ fprintf (f, "\treturn (fsm_%s_func_t) pgm_read_word "
+ "(&fsm_%s_trans_table[event][state]);\n",
+ fsm->name,
+ fsm->name);
+ fprintf (f, "}\n\n");
+
/* Gen active states array. */
fprintf (f, "fsm_%s_state_t fsm_%s_active_states[%u];\n\n",
fsm->name,
@@ -1324,19 +1345,23 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
fprintf (f, "\tint handled = 0;\n");
fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n",
fsm->name);
- fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n",
+ fprintf (f, "\t\tif (fsm_%s_read_trans (e, fsm_%s_active_states[i]))\n",
fsm->name,
fsm->name);
fprintf (f, "\t\t{\n");
- fprintf (f, "\t\t\tfsm_%s_active_states[i] = \
- fsm_%s_trans_table[e][fsm_%s_active_states[i]]();\n",
- fsm->name,
- fsm->name,
+ fprintf (f, "\t\t\tproto_send2b ('F', fsm_%s_active_states[i], e);\n",
fsm->name);
+ fprintf (f, "\t\t\tfsm_%s_active_states[i] = fsm_%s_read_trans (e, "
+ "fsm_%s_active_states[i])();\n",
+ fsm->name,
+ fsm->name,
+ fsm->name);
fprintf (f, "\t\t\thandled = 1;\n");
if (fsm->timeouts != NULL)
{
- fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = fsm_%s_timeout_values[e];\n",
+ fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = "
+ "fsm_%s_timeout_values[fsm_%s_active_states[i]];\n",
+ fsm->name,
fsm->name,
fsm->name);
}
@@ -1352,9 +1377,9 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
fprintf (f, "\tuint16_t i;\n");
fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n",
fsm->name);
- fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n",
- fsm->name,
- fsm->name);
+ fprintf (f, "\t\tif (fsm_%s_read_trans (e, fsm_%s_active_states[i]))\n",
+ fsm->name,
+ fsm->name);
fprintf (f, "\t\t\treturn 1;\n");
fprintf (f, "\treturn 0;\n");
fprintf (f, "}\n\n");
@@ -1431,18 +1456,19 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
fprintf (f, "\tint out = 0;\n");
fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n",
fsm->name);
- fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n",
+ fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n\t\t{\n",
fsm->name);
fprintf (f, "\t\t\tfsm_%s_timeout_counters[i]--;\n",
fsm->name);
- fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t{\n",
+ fprintf (f, "\t\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t\t{\n",
fsm->name);
- fprintf (f, "\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n",
+ fprintf (f, "\t\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n",
fsm->name,
fsm->name,
fsm->name);
- fprintf (f, "\t\t\tout = 1;\n");
- fprintf (f, "\t\t}\n\n");
+ fprintf (f, "\t\t\t\tout = 1;\n");
+ fprintf (f, "\t\t\t}\n");
+ fprintf (f, "\t\t}\n");
fprintf (f, "\t}\n");
fprintf (f, "\treturn out;\n");
fprintf (f, "}\n\n");
diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c
new file mode 100644
index 00000000..51ba31ab
--- /dev/null
+++ b/digital/ai/src/fsm/init.c
@@ -0,0 +1,185 @@
+/* init.c */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+
+#include "asserv.h"
+#include "contact.h"
+#include "chrono.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
+
+#include "bot.h"
+#include "init_defs.h"
+
+/*
+ * Initialise robot position with a calibration procedure.
+ */
+
+FSM_STATES (
+ /* Initial state, waiting for jack. */
+ INIT_START,
+ /* After first jack insertion, waiting for removal to initialise actuators. */
+ INIT_WAITING_FIRST_JACK_OUT,
+ /* Initialising actuators, then waiting for second jack insertion. */
+ INIT_INITIALISING_ACTUATORS,
+ /* After second jack insertion, waiting the operator remove its
+ * hand. */
+ INIT_WAITING_HANDS_OUT,
+ /* Finding the first wall. */
+ INIT_FINDING_FIRST_WALL,
+ /* Going away from the wall. */
+ INIT_GOING_AWAY_FIRST_WALL,
+ /* Turning to face the other wall. */
+ INIT_FACING_SECOND_WALL,
+ /* Waiting after rotation for robot to stabilize. */
+ INIT_WAITING_AFTER_FACING_SECOND_WALL,
+ /* Finding the second wall. */
+ INIT_FINDING_SECOND_WALL,
+ /* Going away from the wall. */
+ INIT_GOING_AWAY_SECOND_WALL,
+#ifdef INIT_START_POSITION_ANGLE
+ /* Facing the start position. */
+ INIT_FACING_START_POSITION,
+#endif
+ /* Going to start position. */
+ INIT_GOING_TO_START_POSITION,
+ /* Waiting for the round start (waiting for the jack). */
+ INIT_WAITING_SECOND_JACK_OUT,
+ /* Initialisation finished, nothing else to do. */
+ INIT_FINISHED)
+
+FSM_EVENTS (
+ /* Jack is inserted. */
+ jack_inserted,
+ /* Jack is removed. */
+ jack_removed,
+ /* Sent to initialise actuators. */
+ init_actuators,
+ /* Sent to start round. */
+ init_start_round)
+
+FSM_START_WITH (INIT_START)
+
+FSM_TRANS (INIT_START, jack_inserted, INIT_WAITING_FIRST_JACK_OUT)
+{
+ return FSM_NEXT (INIT_START, jack_inserted);
+}
+
+FSM_TRANS (INIT_WAITING_FIRST_JACK_OUT, jack_removed,
+ INIT_INITIALISING_ACTUATORS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, init_actuators));
+ return FSM_NEXT (INIT_WAITING_FIRST_JACK_OUT, jack_removed);
+}
+
+FSM_TRANS (INIT_INITIALISING_ACTUATORS, jack_inserted, INIT_WAITING_HANDS_OUT)
+{
+ team_color = contact_get_color ();
+ return FSM_NEXT (INIT_INITIALISING_ACTUATORS, jack_inserted);
+}
+
+FSM_TRANS_TIMEOUT (INIT_WAITING_HANDS_OUT, 225, INIT_FINDING_FIRST_WALL)
+{
+ asserv_set_speed (BOT_SPEED_INIT);
+ asserv_push_the_wall (INIT_FIRST_WALL_PUSH);
+ return FSM_NEXT_TIMEOUT (INIT_WAITING_HANDS_OUT);
+}
+
+FSM_TRANS (INIT_FINDING_FIRST_WALL, robot_move_success,
+ INIT_GOING_AWAY_FIRST_WALL)
+{
+ asserv_move_linearly (INIT_FIRST_WALL_AWAY);
+ return FSM_NEXT (INIT_FINDING_FIRST_WALL, robot_move_success);
+}
+
+FSM_TRANS (INIT_GOING_AWAY_FIRST_WALL, robot_move_success,
+ INIT_FACING_SECOND_WALL)
+{
+ asserv_goto_angle (INIT_SECOND_WALL_ANGLE);
+ return FSM_NEXT (INIT_GOING_AWAY_FIRST_WALL, robot_move_success);
+}
+
+FSM_TRANS (INIT_FACING_SECOND_WALL, robot_move_success,
+ INIT_WAITING_AFTER_FACING_SECOND_WALL)
+{
+ return FSM_NEXT (INIT_FACING_SECOND_WALL, robot_move_success);
+}
+
+FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 225 / 2,
+ INIT_FINDING_SECOND_WALL)
+{
+ asserv_push_the_wall (INIT_SECOND_WALL_PUSH);
+ return FSM_NEXT_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL);
+}
+
+FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success,
+ INIT_GOING_AWAY_SECOND_WALL)
+{
+ asserv_move_linearly (INIT_SECOND_WALL_AWAY);
+ return FSM_NEXT (INIT_FINDING_SECOND_WALL, robot_move_success);
+}
+
+#ifdef INIT_START_POSITION_ANGLE
+FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success,
+ INIT_FACING_START_POSITION)
+{
+ asserv_goto_angle (INIT_START_POSITION_ANGLE);
+ return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success);
+}
+
+FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success,
+ INIT_GOING_TO_START_POSITION)
+{
+ asserv_goto_xya (INIT_START_POSITION);
+ return FSM_NEXT (INIT_FACING_START_POSITION, robot_move_success);
+}
+
+#else
+
+FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success,
+ INIT_GOING_TO_START_POSITION)
+{
+ asserv_goto_xya (INIT_START_POSITION);
+ return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success);
+}
+
+#endif
+
+FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success,
+ INIT_WAITING_SECOND_JACK_OUT)
+{
+ asserv_set_speed (BOT_SPEED_NORMAL);
+ return FSM_NEXT (INIT_GOING_TO_START_POSITION, robot_move_success);
+}
+
+FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED)
+{
+ chrono_init ();
+ fsm_queue_post_event (FSM_EVENT (AI, init_start_round));
+ return FSM_NEXT (INIT_WAITING_SECOND_JACK_OUT, jack_removed);
+}
+
diff --git a/digital/ai/src/move/radar.c b/digital/ai/src/move/radar.c
new file mode 100644
index 00000000..a572afba
--- /dev/null
+++ b/digital/ai/src/move/radar.c
@@ -0,0 +1,168 @@
+/* radar.c */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "radar.h"
+
+#include "bot.h"
+
+#include "modules/math/geometry/geometry.h"
+#include "modules/math/geometry/distance.h"
+#include "modules/utils/utils.h"
+
+/** Maximum distance for a sensor reading to be ignored if another sensor is
+ * nearer. */
+#define RADAR_FAR_MM 250
+
+/** Define radar configuration. */
+extern struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB];
+
+uint8_t
+radar_valid (vect_t p);
+
+/** Compute the center position from several radars sensors, return 1 if
+ * any. */
+static uint8_t
+radar_hit_center (uint8_t valid[], vect_t hit[], uint8_t sensor_nb,
+ vect_t *obs_pos)
+{
+ uint8_t i, hit_nb = 0;
+ vect_t hit_center = { 0, 0 };
+ for (i = 0; i < sensor_nb; i++)
+ {
+ if (valid[i])
+ {
+ vect_add (&hit_center, &hit[i]);
+ hit_nb++;
+ }
+ }
+ if (hit_nb > 1)
+ vect_scale_f824 (&hit_center, 0x1000000l / hit_nb);
+ if (hit_nb)
+ {
+ *obs_pos = hit_center;
+ return 1;
+ }
+ else
+ return 0;
+}
+
+uint8_t
+radar_update (const position_t *robot_pos, vect_t *obs_pos)
+{
+ uint8_t i, j;
+ vect_t ray;
+ uint8_t obs_nb = 0;
+ /* Compute hit points for each sensor and eliminate invalid ones. */
+ vect_t hit[UTILS_COUNT (radar_sensors)];
+ uint8_t valid[UTILS_COUNT (radar_sensors)];
+ uint16_t dist_mm[UTILS_COUNT (radar_sensors)];
+ for (i = 0; i < UTILS_COUNT (radar_sensors); i++)
+ {
+ dist_mm[i] = *radar_sensors[i].dist_mm;
+ if (dist_mm[i] != 0xffff)
+ {
+ hit[i] = radar_sensors[i].pos;
+ vect_rotate_uf016 (&hit[i], robot_pos->a);
+ vect_translate (&hit[i], &robot_pos->v);
+ vect_from_polar_uf016 (&ray, dist_mm[i],
+ robot_pos->a + radar_sensors[i].a);
+ vect_translate (&hit[i], &ray);
+ valid[i] = radar_valid (hit[i]);
+ vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM,
+ robot_pos->a + radar_sensors[i].a);
+ vect_translate (&hit[i], &ray);
+ }
+ else
+ valid[i] = 0;
+ }
+ /* Ignore sensor results too far from other sensors. */
+ for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++)
+ {
+ for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++)
+ {
+ if (valid[j])
+ {
+ if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j])
+ valid[j] = 0;
+ else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i])
+ valid[i] = 0;
+ }
+ }
+ }
+ /* Specific treatment about sensor topology. */
+ obs_nb += radar_hit_center (valid + RADAR_SENSOR_FRONT_FIRST,
+ hit + RADAR_SENSOR_FRONT_FIRST,
+ RADAR_SENSOR_FRONT_NB, &obs_pos[obs_nb]);
+ obs_nb += radar_hit_center (valid + RADAR_SENSOR_BACK_FIRST,
+ hit + RADAR_SENSOR_BACK_FIRST,
+ RADAR_SENSOR_BACK_NB, &obs_pos[obs_nb]);
+ /* Done. */
+ return obs_nb;
+}
+
+uint8_t
+radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
+ const vect_t *obs_pos, uint8_t obs_nb)
+{
+ uint8_t i;
+ /* Stop here if no obstacle. */
+ if (!obs_nb)
+ return 0;
+ vect_t vd = *dest_pos; vect_sub (&vd, robot_pos);
+ uint16_t d = vect_norm (&vd);
+ /* If destination is realy near, stop here. */
+ if (d < RADAR_EPSILON_MM)
+ return 0;
+ /* If destination is near, use clearance to destination point instead of
+ * stop length. */
+ vect_t t;
+ if (d < RADAR_STOP_MM)
+ t = *dest_pos;
+ else
+ {
+ vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM);
+ t = *robot_pos;
+ vect_translate (&t, &vd);
+ }
+ /* Now, look at obstacles. */
+ for (i = 0; i < obs_nb; i++)
+ {
+ /* Vector from robot to obstacle. */
+ vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos);
+ /* Ignore if in our back. */
+ int32_t dp = vect_dot_product (&vd, &vo);
+ if (dp < 0)
+ continue;
+ /* Check distance. */
+ int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]);
+ if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2
+ + RADAR_OBSTACLE_RADIUS_MM)
+ continue;
+ /* Else, obstacle is blocking. */
+ return 1;
+ }
+ return 0;
+}
+
diff --git a/digital/ai/src/move/radar.h b/digital/ai/src/move/radar.h
new file mode 100644
index 00000000..2e37d11d
--- /dev/null
+++ b/digital/ai/src/move/radar.h
@@ -0,0 +1,96 @@
+#ifndef radar_h
+#define radar_h
+/* radar.h */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "defs.h"
+
+/**
+ * Handle any distance sensors information to extract useful data. This
+ * includes:
+ * - combining several sensors information for a more precise obstacle
+ * position,
+ * - ignoring obstacles not in the playground,
+ * - determining if an obstacle should make the robot stop.
+ */
+
+/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is
+ * added to position obstacle center. */
+#undef RADAR_OBSTACLE_EDGE_RADIUS_MM
+
+/** Estimated obstacle radius. The obstacle may be larger than at the
+ * detected edge. */
+#undef RADAR_OBSTACLE_RADIUS_MM
+
+/** Stop distance. Distance under which an obstacle is considered harmful when
+ * moving. */
+#undef RADAR_STOP_MM
+
+/** Clearance distance. Distance over which an obstacle should be to the side
+ * when moving.
+ *
+ * OK, more explanations: when moving, a rectangle is placed in front of the
+ * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM +
+ * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered
+ * in the way.
+ *
+ * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM),
+ * this reduce the rectangle length.
+ *
+ * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */
+#undef RADAR_CLEARANCE_MM
+
+/** Destination distance near enough so that obstacles could be ignored. */
+#undef RADAR_EPSILON_MM
+
+/* All this in another file. */
+#include "radar_defs.h"
+
+/** Margin to be considered inside the playground. An obstacle can not be
+ * exactly at the playground edge. */
+#define RADAR_MARGIN_MM 150
+
+/** Describe a radar sensor. */
+struct radar_sensor_t
+{
+ /** Distance updated by another module. */
+ uint16_t *dist_mm;
+ /** Position relative to the robot center. */
+ vect_t pos;
+ /** Angle relative to the robot X axis. */
+ uint16_t a;
+};
+
+/** Update radar view. Return the number of obstacles found. Obstacles
+ * positions are returned in obs_pos. */
+uint8_t
+radar_update (const position_t *robot_pos, vect_t *obs_pos);
+
+/** Return non zero if there is a blocking obstacle near the robot while going
+ * to a destination point. */
+uint8_t
+radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
+ const vect_t *obs_pos, uint8_t obs_nb);
+
+#endif /* radar_h */
diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
index d066737b..839b1d23 100644
--- a/digital/ai/src/twi_master/asserv.c
+++ b/digital/ai/src/twi_master/asserv.c
@@ -202,12 +202,12 @@ asserv_get_moving_direction (void)
{
/* Foward move? */
if (asserv_status.status & _BV (asserv_status_flag_move_forward))
- return 1;
+ return DIRECTION_FORWARD;
/* Backward move? */
if (asserv_status.status & _BV (asserv_status_flag_move_backward))
- return 2;
+ return DIRECTION_BACKWARD;
/* Not moving */
- return 0;
+ return DIRECTION_NONE;
}
uint8_t
@@ -301,6 +301,28 @@ asserv_go_to_the_wall (uint8_t backward)
}
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)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
index 0d29ba96..c9a52a11 100644
--- a/digital/ai/src/twi_master/asserv.h
+++ b/digital/ai/src/twi_master/asserv.h
@@ -121,16 +121,16 @@ asserv_get_motor1_position (void);
/**
* Are we moving forward/backward?
* @return
- * - 0 we are not moving;
- * - 1 we are moving forward;
- * - 2 we are moving backward.
+ * - DIRECTION_NONE we are not moving;
+ * - DIRECTION_FORWARD we are moving forward;
+ * - DIRECTION_BACKWARD we are moving backward.
*/
uint8_t
asserv_get_moving_direction (void);
/**
* Get the last moving direction of the bot.
- * @return 1 is forward, 2 is backward.
+ * @return DIRECTION_FORWARD or DIRECTION_BACKWARD.
*/
uint8_t
asserv_get_last_moving_direction (void);
@@ -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/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
index af6ff5f9..8df84538 100644
--- a/digital/ai/src/twi_master/mimot.c
+++ b/digital/ai/src/twi_master/mimot.c
@@ -97,6 +97,12 @@ mimot_motor1_cmd_status (void)
return none;
}
+uint8_t
+mimot_get_input (void)
+{
+ return mimot_status.input_port;
+}
+
uint16_t
mimot_get_motor0_position (void)
{
@@ -118,6 +124,30 @@ mimot_reset (void)
}
void
+mimot_set_motor0_position (uint16_t position)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'Y';
+ buffer[2] = 0;
+ buffer[3] = v16_to_v8 (position, 1);
+ buffer[4] = v16_to_v8 (position, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+mimot_set_motor1_position (uint16_t position)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'Y';
+ buffer[2] = 1;
+ buffer[3] = v16_to_v8 (position, 1);
+ buffer[4] = v16_to_v8 (position, 0);
+ twi_master_send_buffer (5);
+}
+
+void
mimot_move_motor0_absolute (uint16_t position, uint8_t speed)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
@@ -142,19 +172,41 @@ mimot_move_motor1_absolute (uint16_t position, uint8_t speed)
void
mimot_motor0_zero_position (int8_t speed)
{
+ mimot_motor0_find_zero (speed, 0, 0);
+}
+
+void
+mimot_motor1_zero_position (int8_t speed)
+{
+ mimot_motor1_find_zero (speed, 0, 0);
+}
+
+void
+mimot_motor0_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position)
+{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'B';
- buffer[1] = speed;
- twi_master_send_buffer (2);
+ buffer[1] = 0;
+ buffer[2] = speed;
+ buffer[3] = use_switch;
+ buffer[4] = v16_to_v8 (reset_position, 1);
+ buffer[5] = v16_to_v8 (reset_position, 0);
+ twi_master_send_buffer (6);
}
void
-mimot_motor1_zero_position (int8_t speed)
+mimot_motor1_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'C';
- buffer[1] = speed;
- twi_master_send_buffer (2);
+ buffer[0] = 'B';
+ buffer[1] = 1;
+ buffer[2] = speed;
+ buffer[3] = use_switch;
+ buffer[4] = v16_to_v8 (reset_position, 1);
+ buffer[5] = v16_to_v8 (reset_position, 0);
+ twi_master_send_buffer (6);
}
void
@@ -181,3 +233,21 @@ mimot_motor1_clamp (int8_t speed, int16_t pwm)
twi_master_send_buffer (5);
}
+void
+mimot_motor0_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'w';
+ buffer[1] = 0;
+ twi_master_send_buffer (2);
+}
+
+void
+mimot_motor1_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'w';
+ buffer[1] = 1;
+ twi_master_send_buffer (2);
+}
+
diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h
index 60811c10..c9de7090 100644
--- a/digital/ai/src/twi_master/mimot.h
+++ b/digital/ai/src/twi_master/mimot.h
@@ -56,6 +56,10 @@ mimot_motor0_cmd_status (void);
asserv_status_e
mimot_motor1_cmd_status (void);
+/** Return input port state. */
+uint8_t
+mimot_get_input (void);
+
/** Get motor0 position in steps. */
uint16_t
mimot_get_motor0_position (void);
@@ -68,6 +72,14 @@ mimot_get_motor1_position (void);
void
mimot_reset (void);
+/** Set motor0 position in steps. */
+void
+mimot_set_motor0_position (uint16_t position);
+
+/** Set motor1 position in steps. */
+void
+mimot_set_motor1_position (uint16_t position);
+
/** Move motor0 to absolute position in steps. */
void
mimot_move_motor0_absolute (uint16_t position, uint8_t speed);
@@ -84,6 +96,16 @@ mimot_motor0_zero_position (int8_t speed);
void
mimot_motor1_zero_position (int8_t speed);
+/** Find zero position. */
+void
+mimot_motor0_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position);
+
+/** Find zero position. */
+void
+mimot_motor1_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position);
+
/** Clamp motor0. */
void
mimot_motor0_clamp (int8_t speed, int16_t pwm);
@@ -92,4 +114,12 @@ mimot_motor0_clamp (int8_t speed, int16_t pwm);
void
mimot_motor1_clamp (int8_t speed, int16_t pwm);
+/** Free motor0. */
+void
+mimot_motor0_free (void);
+
+/** Free motor1. */
+void
+mimot_motor1_free (void);
+
#endif /* mimot_h */
diff --git a/digital/ai/src/utils/chrono.c b/digital/ai/src/utils/chrono.c
index a1212e19..85851547 100644
--- a/digital/ai/src/utils/chrono.c
+++ b/digital/ai/src/utils/chrono.c
@@ -148,3 +148,10 @@ chrono_end_match (uint8_t block)
;
#endif
}
+
+void
+chrono_set_timer (uint32_t elapsed_time)
+{
+ if (chrono_enabled_)
+ chrono_ov_count_ = elapsed_time / TIMER_PERIOD_MS;
+}
diff --git a/digital/ai/src/utils/chrono.h b/digital/ai/src/utils/chrono.h
index 38bf3d61..580529cf 100644
--- a/digital/ai/src/utils/chrono.h
+++ b/digital/ai/src/utils/chrono.h
@@ -96,4 +96,12 @@ chrono_remaining_time (void);
void
chrono_end_match (uint8_t block);
+/**
+ * Set timer at desired value.
+ * This function should be used for tests purpose only.
+ * @param elapsed_time elapsed time since beginning.
+ */
+void
+chrono_set_timer (uint32_t elapsed_time);
+
#endif /* chrono_h */
diff --git a/digital/ai/tools/marcel.py b/digital/ai/tools/marcel.py
index 08314ca3..001eca5e 100644
--- a/digital/ai/tools/marcel.py
+++ b/digital/ai/tools/marcel.py
@@ -1,10 +1,3 @@
-import simu.model.table_eurobot2010
-import simu.view.table_eurobot2010
-
-import simu.robots.marcel.link.bag
-import simu.robots.marcel.model.bag
-import simu.robots.marcel.view.bag
-
import asserv
import asserv.init
import mimot
@@ -16,24 +9,37 @@ from proto.popen_io import PopenIO
import math
class Robot:
+ """Marcel robot instance."""
+
+ import simu.model.table_eurobot2010 as table_model
+ import simu.view.table_eurobot2010 as table_view
+
+ import simu.robots.marcel.link.bag as robot_link
+ import simu.robots.marcel.model.bag as robot_model
+ import simu.robots.marcel.view.bag as robot_view
+
+ robot_start_pos = {
+ False: (300, 2100 - 305, math.radians (-270)),
+ True: (3000 - 300, 2100 - 305, math.radians (-270))
+ }
+
+ client_nb = 3
- def __init__ (self, proto_time):
- self.table_model = simu.model.table_eurobot2010
- self.table_view = simu.view.table_eurobot2010
- self.robot_link = simu.robots.marcel.link.bag
- self.robot_model = simu.robots.marcel.model.bag
- self.robot_view = simu.robots.marcel.view.bag
- asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel')
- mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel')
- io_cmd = ('../../io/src/io.host')
- self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time,
- **asserv.init.host['marcel'])
- self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time,
- **mimot.init.host['marcel'])
- self.io = io.Proto (PopenIO (io_cmd), proto_time,
- **io.init.host['marcel'])
- self.robot_start_pos = {
- False: (300, 2100 - 305, math.radians (-270)),
- True: (3000 - 300, 2100 - 305, math.radians (-270))
- }
+ def __init__ (self, proto_time, instance = 'robot0'):
+ self.instance = instance
+ def proto (proto_class, cmd, init):
+ cmd = [ s.format (instance = instance) for s in cmd ]
+ return proto_class (PopenIO (cmd), proto_time, **init)
+ asserv_cmd = ('../../asserv/src/asserv/asserv.host',
+ '-i{instance}:asserv0', '-m9', 'marcel')
+ mimot_cmd = ('../../mimot/src/dirty/dirty.host',
+ '-i{instance}:mimot0', '-m9', 'marcel')
+ io_cmd = ('../../io/src/io.host', '-i{instance}:io0')
+ self.asserv = proto (asserv.Proto, asserv_cmd,
+ asserv.init.host['marcel'])
+ self.mimot = proto (mimot.Proto, mimot_cmd,
+ mimot.init.host['marcel'])
+ self.io = proto (io.Proto, io_cmd,
+ io.init.host['marcel'])
+ self.protos = (self.asserv, self.mimot, self.io)
diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py
index 09be4bea..0d258b34 100644
--- a/digital/ai/tools/robospierre.py
+++ b/digital/ai/tools/robospierre.py
@@ -1,10 +1,3 @@
-import simu.model.table_eurobot2011
-import simu.view.table_eurobot2011
-
-import simu.robots.robospierre.link.bag
-import simu.robots.robospierre.model.bag
-import simu.robots.robospierre.view.bag
-
import asserv
import asserv.init
import mimot
@@ -16,24 +9,39 @@ from proto.popen_io import PopenIO
import math
class Robot:
+ """Robospierre robot instance."""
+
+ import simu.model.table_eurobot2011 as table_model
+ import simu.view.table_eurobot2011 as table_view
+
+ import simu.robots.robospierre.link.bag as robot_link
+ import simu.robots.robospierre.model.bag as robot_model
+ import simu.robots.robospierre.view.bag as robot_view
+
+ robot_start_pos = {
+ # In real life, better place the robot in green zone.
+ False: (300, 2100 - 200, math.radians (180)),
+ True: (3000 - 300, 2100 - 200, math.radians (0))
+ }
+
+ client_nb = 3
- def __init__ (self, proto_time):
- self.table_model = simu.model.table_eurobot2011
- self.table_view = simu.view.table_eurobot2011
- self.robot_link = simu.robots.robospierre.link.bag
- self.robot_model = simu.robots.robospierre.model.bag
- self.robot_view = simu.robots.robospierre.view.bag
- asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel')
- mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel')
- io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host')
- self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time,
- **asserv.init.host['robospierre'])
- self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time,
- **mimot.init.host['robospierre'])
- self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time,
- **io_hub.init.host['robospierre'])
- self.robot_start_pos = {
- False: (700, 2100 - 250, math.radians (-270)),
- True: (3000 - 700, 2100 - 250, math.radians (-270))
- }
+ def __init__ (self, proto_time, instance = 'robot0'):
+ self.instance = instance
+ def proto (proto_class, cmd, init):
+ cmd = [ s.format (instance = instance) for s in cmd ]
+ return proto_class (PopenIO (cmd), proto_time, **init)
+ asserv_cmd = ('../../asserv/src/asserv/asserv.host',
+ '-i{instance}:asserv0', '-m9', 'robospierre')
+ mimot_cmd = ('../../mimot/src/dirty/dirty.host',
+ '-i{instance}:mimot0', '-m9', 'robospierre')
+ io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host',
+ '-i{instance}:io0')
+ self.asserv = proto (asserv.Proto, asserv_cmd,
+ asserv.init.host['robospierre'])
+ self.mimot = proto (mimot.Proto, mimot_cmd,
+ mimot.init.host['robospierre'])
+ self.io = proto (io_hub.Proto, io_hub_cmd,
+ io_hub.init.host['robospierre'])
+ self.protos = (self.asserv, self.mimot, self.io)
diff --git a/digital/ai/tools/test_simu.py b/digital/ai/tools/test_simu.py
index 80008bb9..1b804e22 100644
--- a/digital/ai/tools/test_simu.py
+++ b/digital/ai/tools/test_simu.py
@@ -49,32 +49,29 @@ class ObstacleWithBeacon (obstacle_view.RoundObstacle):
class TestSimu (InterNode):
"""Interface, with simulated programs."""
- def __init__ (self, robot_class):
+ def __init__ (self, robot_class, robot_nb = 1, color_switch = True):
# Hub.
- self.hub = mex.hub.Hub (min_clients = 4)
+ self.hub = mex.hub.Hub (min_clients = 1 + robot_class.client_nb
+ * robot_nb)
self.forked_hub = utils.forked.Forked (self.hub.wait)
# InterNode.
InterNode.__init__ (self)
def proto_time ():
return self.node.date / self.node.tick
- # Robot parameters.
- robot = robot_class (proto_time)
- self.robot = robot
- # Asserv.
- self.asserv = robot.asserv
- self.asserv.async = True
- self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read)
- # Mimot.
- self.mimot = robot.mimot
- self.mimot.async = True
- self.tk.createfilehandler (self.mimot, READABLE, self.mimot_read)
- # Io.
- self.io = robot.io
- self.io.async = True
- self.tk.createfilehandler (self.io, READABLE, self.io_read)
+ # Robot instances.
+ self.robots = [ robot_class (proto_time, 'robot%d' % i)
+ for i in xrange (robot_nb) ]
+ for r in self.robots:
+ for prog in r.protos:
+ prog.async = True
+ def prog_read (f, mask, prog = prog):
+ prog.proto.read ()
+ prog.proto.sync ()
+ self.tk.createfilehandler (prog, READABLE, prog_read)
# Add table.
- self.table_model = robot.table_model.Table ()
- self.table = robot.table_view.Table (self.table_view, self.table_model)
+ self.table_model = robot_class.table_model.Table ()
+ self.table = robot_class.table_view.Table (self.table_view,
+ self.table_model)
self.obstacle = obstacle_model.RoundObstacle (150)
self.table_model.obstacles.append (self.obstacle)
self.obstacle_beacon = obstacle_model.RoundObstacle (40, 2)
@@ -82,43 +79,31 @@ class TestSimu (InterNode):
self.obstacle_view = ObstacleWithBeacon (self.table, self.obstacle,
self.obstacle_beacon)
self.table_view.bind ('<2>', self.place_obstacle)
- # Add robot.
- self.robot_link = robot.robot_link.Bag (self.node)
- self.robot_model = robot.robot_model.Bag (self.node, self.table_model,
- self.robot_link)
- self.robot_view = robot.robot_view.Bag (self.table,
- self.actuator_view, self.sensor_frame, self.robot_model)
- # Color switch.
- self.robot_model.color_switch.register (self.change_color)
+ # Add robots.
+ for r in self.robots:
+ r.link = r.robot_link.Bag (self.node, r.instance)
+ r.model = r.robot_model.Bag (self.node, self.table_model, r.link)
+ r.view = r.robot_view.Bag (self.table, self.actuator_view,
+ self.sensor_frame, r.model)
+ # Color switch.
+ if color_switch:
+ def change_color (r = r):
+ i = r.model.color_switch.state
+ r.asserv.set_simu_pos (*r.robot_start_pos[i])
+ r.model.color_switch.register (change_color)
def close (self):
self.forked_hub.kill ()
import time
time.sleep (1)
- def asserv_read (self, file, mask):
- self.asserv.proto.read ()
- self.asserv.proto.sync ()
-
- def mimot_read (self, file, mask):
- self.mimot.proto.read ()
- self.mimot.proto.sync ()
-
- def io_read (self, file, mask):
- self.io.proto.read ()
- self.io.proto.sync ()
-
def step (self):
"""Overide step to handle retransmissions, could be made cleaner using
simulated time."""
InterNode.step (self)
- self.asserv.proto.sync ()
- self.mimot.proto.sync ()
- self.io.proto.sync ()
-
- def change_color (self, *dummy):
- i = self.robot_model.color_switch.state
- self.asserv.set_simu_pos (*self.robot.robot_start_pos[i]);
+ for r in self.robots:
+ for prog in r.protos:
+ prog.proto.sync ()
def place_obstacle (self, ev):
pos = self.table_view.screen_coord ((ev.x, ev.y))
@@ -132,6 +117,8 @@ def run (default_robot, test_class = TestSimu):
parser = optparse.OptionParser ()
parser.add_option ('-r', '--robot', help = "use specified robot",
metavar = 'NAME', default = default_robot)
+ parser.add_option ('-n', '--robot-nb', help = "number of robots",
+ type = 'int', metavar = 'NB', default = 1)
(options, args) = parser.parse_args ()
if args:
parser.error ("too many arguments")
@@ -143,7 +130,7 @@ def run (default_robot, test_class = TestSimu):
robot = robospierre.Robot
else:
parser.error ("unknown robot")
- app = test_class (robot)
+ app = test_class (robot, options.robot_nb)
app.mainloop ()
app.close ()
diff --git a/digital/ai/tools/test_simu_control_marcel.py b/digital/ai/tools/test_simu_control_marcel.py
index 2a43b7bb..1d5563e0 100644
--- a/digital/ai/tools/test_simu_control_marcel.py
+++ b/digital/ai/tools/test_simu_control_marcel.py
@@ -28,8 +28,11 @@ import math
class TestSimuControl (TestSimu):
"""Interface with extra control."""
- def __init__ (self, robot_class):
- TestSimu.__init__ (self, robot_class)
+ def __init__ (self, robot_class, *args):
+ TestSimu.__init__ (self, robot_class, *args, color_switch = False)
+ self.io = self.robots[0].io
+ self.asserv = self.robots[0].asserv
+ self.mimot = self.robots[0].mimot
def create_widgets (self):
TestSimu.create_widgets (self)
diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py
index b1cccf4b..c1a16c3c 100644
--- a/digital/ai/tools/test_simu_control_robospierre.py
+++ b/digital/ai/tools/test_simu_control_robospierre.py
@@ -30,10 +30,13 @@ class TestSimuControl (TestSimu):
ELEVATION_STROKE = 0x3b0b
- ROTATION_STROKE = 0x11c6
+ ROTATION_STROKE = 0x233e
- def __init__ (self, robot_class):
- TestSimu.__init__ (self, robot_class)
+ def __init__ (self, robot_class, *args):
+ TestSimu.__init__ (self, robot_class, *args, color_switch = False)
+ self.io = self.robots[0].io
+ self.asserv = self.robots[0].asserv
+ self.mimot = self.robots[0].mimot
def create_widgets (self):
TestSimu.create_widgets (self)
@@ -46,6 +49,12 @@ class TestSimuControl (TestSimu):
indicatoron = False,
variable = self.clamp_var, command = self.clamp_command)
self.clamp_button.pack ()
+ self.doors_var = IntVar ()
+ self.doors_var.set (1)
+ self.doors_button = Checkbutton (self.control_frame, text = 'Doors',
+ indicatoron = False,
+ variable = self.doors_var, command = self.doors_command)
+ self.doors_button.pack ()
self.elevation_up_button = Button (self.control_frame,
text = 'Elevation up', padx = 0, pady = 0,
command = self.elevation_up_command)
@@ -69,12 +78,35 @@ class TestSimuControl (TestSimu):
text = 'Move clamp', padx = 0, pady = 0,
command = self.clamp_move_command)
self.clamp_move_button.pack ()
+ self.clamp_to_scale = Scale (self.control_frame, orient = HORIZONTAL,
+ from_ = 0, to = 6)
+ self.clamp_to_scale.pack ()
+ self.clamp_element_move_button = Button (self.control_frame,
+ text = 'Move element', padx = 0, pady = 0,
+ command = self.clamp_move_element_command)
+ self.clamp_element_move_button.pack ()
+ self.drop_var = IntVar ()
+ self.drop_button = Checkbutton (self.control_frame, text = 'Drop',
+ indicatoron = False,
+ variable = self.drop_var, command = self.drop_command)
+ self.drop_button.pack ()
+ self.backward_var = IntVar ()
+ self.backward_button = Checkbutton (self.control_frame,
+ text = 'Backward', variable = self.backward_var)
+ self.backward_button.pack ()
+ self.goto_var = IntVar ()
+ self.goto_button = Checkbutton (self.control_frame,
+ text = 'Goto FSM', variable = self.goto_var)
+ self.goto_button.pack ()
self.table_view.bind ('<1>', self.move)
self.table_view.bind ('<3>', self.orient)
def move (self, ev):
pos = self.table_view.screen_coord ((ev.x, ev.y))
- self.asserv.goto (pos[0], pos[1])
+ if self.goto_var.get ():
+ self.io.goto (pos[0], pos[1], self.backward_var.get ())
+ else:
+ self.asserv.goto (pos[0], pos[1], self.backward_var.get ())
def orient (self, ev):
x, y = self.table_view.screen_coord ((ev.x, ev.y))
@@ -84,10 +116,7 @@ class TestSimuControl (TestSimu):
self.asserv.goto_angle (a)
def clamp_command (self):
- if self.clamp_var.get ():
- self.io.pwm_set_timed (0, -0x3ff, 255, 0)
- else:
- self.io.pwm_set_timed (0, 0x3ff, 255, 0)
+ self.io.clamp_openclose (not self.clamp_var.get ())
def elevation_up_command (self):
self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2)
@@ -104,6 +133,24 @@ class TestSimuControl (TestSimu):
def clamp_move_command (self):
self.io.clamp_move (self.clamp_pos_scale.get ())
+ def clamp_move_element_command (self):
+ self.io.clamp_move_element (self.clamp_pos_scale.get (),
+ self.clamp_to_scale.get ())
+
+ def doors_command (self):
+ for i in (0, 2, 3, 5):
+ self.io.door (i, not self.doors_var.get ())
+
+ def drop_command (self):
+ if self.drop_var.get ():
+ if self.backward_var.get ():
+ order = 'drop_backward'
+ else:
+ order = 'drop_forward'
+ else:
+ order = 'drop_clear'
+ self.io.drop (order)
+
def change_color (self, *dummy):
pass