summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
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/io-hub/src/robospierre
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/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/Makefile16
-rw-r--r--digital/io-hub/src/robospierre/avrconfig.h26
-rw-r--r--digital/io-hub/src/robospierre/bot.h147
-rw-r--r--digital/io-hub/src/robospierre/clamp.c876
-rw-r--r--digital/io-hub/src/robospierre/clamp.h50
-rw-r--r--digital/io-hub/src/robospierre/codebar.avr.c64
-rw-r--r--digital/io-hub/src/robospierre/codebar.h36
-rw-r--r--digital/io-hub/src/robospierre/codebar.host.c56
-rw-r--r--digital/io-hub/src/robospierre/contact_defs.h24
-rw-r--r--digital/io-hub/src/robospierre/element.c828
-rw-r--r--digital/io-hub/src/robospierre/element.h213
-rw-r--r--digital/io-hub/src/robospierre/init_defs.h50
-rw-r--r--digital/io-hub/src/robospierre/logistic.c622
-rw-r--r--digital/io-hub/src/robospierre/logistic.h143
-rw-r--r--digital/io-hub/src/robospierre/main.c173
-rw-r--r--digital/io-hub/src/robospierre/main.h31
-rw-r--r--digital/io-hub/src/robospierre/move.c541
-rw-r--r--digital/io-hub/src/robospierre/move.h63
-rw-r--r--digital/io-hub/src/robospierre/path.c621
-rw-r--r--digital/io-hub/src/robospierre/path.h78
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.c222
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.h45
-rw-r--r--digital/io-hub/src/robospierre/playground_2011.h33
-rw-r--r--digital/io-hub/src/robospierre/radar_defs.c46
-rw-r--r--digital/io-hub/src/robospierre/radar_defs.h41
-rw-r--r--digital/io-hub/src/robospierre/simu.host.c45
-rw-r--r--digital/io-hub/src/robospierre/simu.host.h12
-rw-r--r--digital/io-hub/src/robospierre/test_element.c376
-rw-r--r--digital/io-hub/src/robospierre/top.c476
29 files changed, 5892 insertions, 62 deletions
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index 21411631..88e6c79e 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -2,18 +2,24 @@
BASE = ../../../avr
# Name of the program to build.
PROGS = io_hub
+HOST_PROGS = test_element
# Sources to compile.
-io_hub_SOURCES = main.c \
- clamp.c \
- fsm.host.c fsm_AI_gen.avr.c \
+io_hub_SOURCES = main.c top.c \
+ clamp.c logistic.c element.c pawn_sensor.c \
+ codebar.avr.c codebar.host.c \
+ radar_defs.c radar.c path.c move.c \
+ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
pwm.avr.c pwm.host.c \
contact.avr.c contact.host.c \
twi_master.c asserv.c mimot.c \
chrono.c timer.avr.c simu.host.c
+test_element_SOURCES = test_element.c logistic.c element.c
# Modules needed for IO.
MODULES = proto uart twi utils \
- math/fixed math/geometry
-AI_MODULES = twi_master common utils fsm
+ adc devices/usdist \
+ math/fixed math/geometry path/astar
+AI_MODULES = twi_master common utils fsm move
+test_element_MODULES = host math/fixed math/geometry
# Configuration file.
CONFIGFILE = avrconfig.h
AVR_MCU = at90usb1287
diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h
index 0fc2f11d..581fa136 100644
--- a/digital/io-hub/src/robospierre/avrconfig.h
+++ b/digital/io-hub/src/robospierre/avrconfig.h
@@ -100,6 +100,32 @@
/** Use internal pull up. */
#define AC_TWI_PULL_UP 0
+/* usdist - Analog US distance sensor. */
+/** Number of sensors. */
+#define AC_USDIST_NB 4
+/** Measuring period, in number of update call. */
+#define AC_USDIST_PERIOD 1
+/** List of space separated sensor definition, see usdist.h. */
+#define AC_USDIST_SENSORS \
+ USDIST_SENSOR (0, A, 0) \
+ USDIST_SENSOR (1, A, 1) \
+ USDIST_SENSOR (2, A, 2) \
+ USDIST_SENSOR (3, A, 3)
+
+/* path - Path finding module. */
+/** Report path found for debug. */
+#define AC_PATH_REPORT defined (HOST)
+/** Report function name. */
+#define AC_PATH_REPORT_CALLBACK simu_send_path
+/** Number of possible obstacles. */
+#define AC_PATH_OBSTACLES_NB 2
+
+/* astar - A* path finding module. */
+/** Neighbor callback. */
+#define AC_ASTAR_NEIGHBOR_CALLBACK path_astar_neighbor_callback
+/** Heuristic callback. */
+#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback
+
/* io-hub - io/ai board. */
/** TWI address of the io board. */
#define AC_IO_TWI_ADDRESS 10
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index 9043a62c..499046e2 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -31,9 +31,46 @@
#ifdef HOST
# define BOT_SCALE 0.0395840674352314
#else
-# define BOT_SCALE 0.0415178942124
+# define BOT_SCALE 0.0317975134344
#endif
+/** Distance from the robot axis to the front. */
+#define BOT_SIZE_FRONT 150
+/** Distance from the robot axis to the back. */
+#define BOT_SIZE_BACK 150
+/** Distance from the robot axis to the side. */
+#define BOT_SIZE_SIDE 190
+
+/** Radius of an element. */
+#define BOT_ELEMENT_RADIUS 100
+
+/** Distance between the front contact point and the robot center. */
+#define BOT_FRONT_CONTACT_DIST_MM 150
+/** Angle error at the front contact point. */
+#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0
+
+/** Distance from robot center to front pawn detection threshold. */
+#define BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM 190
+/** Distance from robot center to back pawn detection threshold. */
+#define BOT_PAWN_BACK_DETECTION_THRESHOLD_MM -190
+/** Distance from robot center to an element near enough to be taken. */
+#define BOT_PAWN_TAKING_DISTANCE_MM 150
+
+/** Distance from border to position in front of a green element. */
+#define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600
+/** Distance from border to go to capture a green element. */
+#define BOT_GREEN_ELEMENT_DISTANCE_MM \
+ (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 10)
+
+/** Speed used for initialisation. */
+#ifdef HOST
+# define BOT_SPEED_INIT 0x20, 0x20, 0x20, 0x20
+#else
+# define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10
+#endif
+/** Normal cruise speed. */
+#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20
+
#ifdef HOST
# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0
@@ -43,34 +80,106 @@
# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2)
# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b
# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b
-# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2)
-# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2)
+# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000)
+# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000)
# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2)
+#define BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP \
+ BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP
+
+# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0
+# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0
+# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0
+# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e
+# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e
+# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e
-# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0
-# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x11c6
-# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x11c6 / 2)
+# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \
+ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP
+# define BOT_CLAMP_BAY_BACK_ROTATION_STEP \
+ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP
+# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \
+ (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2)
+# define BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP 1000
+
+#define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET 0
+#define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET 0
+#define BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET 0
#else /* !HOST */
# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0
-# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP 0x1d83
-# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3288
-# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0
-# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP 0x1d83
-# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3288
-# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3288
-# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1d83
-# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1d83
-# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP 0x1d83
+# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x1da7 - 250)
+# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x35e2
+# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169
+# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250)
+# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610
+# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP (0x3596 + 3 * 0x72)
+# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7
+# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03
+# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2)
+#define BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP 0x363f
+
+# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 92
+# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 92
+# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 92
+# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x2433
+# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x2433
+# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x2433
-# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0
-# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x10e2
-# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x820
+# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \
+ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP
+# define BOT_CLAMP_BAY_BACK_ROTATION_STEP \
+ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP
+# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x1183 + 120)
+# define BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP 1000
+
+#define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129
+#define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60
+#define BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET -120
#endif /* !HOST */
+#define BOT_CLAMP_INIT_ELEVATION_STEP \
+ (BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + 10 * 0x72)
+
+#define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \
+ (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \
+ ? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \
+ : (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \
+ ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET \
+ : BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET))
+
+#define BOT_CLAMP_INIT_ELEVATION_SPEED 0x10
+#define BOT_CLAMP_INIT_ROTATION_SPEED -0x04
#define BOT_CLAMP_ELEVATION_SPEED 0x60
-#define BOT_CLAMP_ROTATION_SPEED 0x30
+#define BOT_CLAMP_ROTATION_SPEED 0x60
+#define BOT_CLAMP_ROTATION_OFFSET_SPEED 1
+
+#define BOT_PWM_CLAMP 2
+#define BOT_PWM_DOOR_FRONT_BOTTOM 0
+#define BOT_PWM_DOOR_FRONT_TOP 1
+#define BOT_PWM_DOOR_BACK_BOTTOM 3
+#define BOT_PWM_DOOR_BACK_TOP 4
+
+#define BOT_PWM_CLAMP_OPEN_TIME 150
+#define BOT_PWM_CLAMP_OPEN 0x1ff, 150, 0
+#define BOT_PWM_CLAMP_CLOSE_TIME 150
+#define BOT_PWM_CLAMP_CLOSE -0x1ff, 150, 0
+
+#define BOT_PWM_DOOR_OPEN_TIME 12
+#define BOT_PWM_DOOR_OPEN(slot) \
+ 0x1ff, (((slot) == CLAMP_SLOT_FRONT_BOTTOM \
+ || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? 80 : 62), 0x55
+#define BOT_PWM_DOOR_CLOSE_TIME 100
+#define BOT_PWM_DOOR_CLOSE(slot) \
+ -0x1ff, (((slot) == CLAMP_SLOT_FRONT_BOTTOM \
+ || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? 80 : 62), \
+ (((slot) == CLAMP_SLOT_FRONT_BOTTOM \
+ || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x200)
+
+#define BOT_PWM_CLAMP_INIT 0x1ff, 150, 0
+#define BOT_PWM_DOOR_INIT 0x100, 160, 0
+#define BOT_PWM_DOOR_INIT_START 0x55
+#define BOT_PWM_CLAMP_DOOR_INIT 250
#endif /* bot_h */
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 2f1b2471..84b2771c 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -25,39 +25,145 @@
#include "common.h"
#include "clamp.h"
+#include "mimot.h"
+#include "pwm.h"
+#include "contact.h"
+#include "bot.h"
+#include "element.h"
+#include "playground.h"
+
#define FSM_NAME AI
#include "fsm.h"
+#include "fsm_queue.h"
-#include "mimot.h"
-#include "bot.h"
+#include "logistic.h"
+#include "pawn_sensor.h"
-FSM_INIT
+/*
+ * There is two FSM in this file.
+ *
+ * The clamp FSM handles high level clamp behaviour, new elements, drop, and
+ * gives orders to the clamp move FSM.
+ *
+ * The clamp move FSM only handle moving the clamp without load or moving an
+ * element from a slot to another one.
+ */
FSM_STATES (
- /* Wait order. */
+ /* Initial state. */
+ CLAMP_START,
+ /* Initialisation sequence: opening everything. */
+ CLAMP_INIT_OPENING,
+ /* Initialisation sequence: going to the middle level. */
+ CLAMP_INIT_GOING_MIDDLE,
+ /* Initialisation sequence: finding front right edge. */
+ CLAMP_INIT_FINDING_ROTATION_EDGE,
+ /* Initialisation sequence: finding top switch. */
+ CLAMP_INIT_FINDING_TOP,
+ /* Initialisation sequence: going to rest position. */
+ CLAMP_INIT_GOING_REST,
+ /* Clamp ready, waiting in rest position. */
+ CLAMP_INIT_READY,
+
+ /* Returning to idle position. */
+ CLAMP_GOING_IDLE,
+ /* Waiting external events, clamp at middle level. */
CLAMP_IDLE,
+ /* Taking an element at bottom slots. */
+ CLAMP_TAKING_DOOR_CLOSING,
+ /* Moving elements around. */
+ CLAMP_MOVING_ELEMENT,
+ /* Droping a tower. */
+ CLAMP_DROPING_DOOR_OPENING,
+ /* Droping a tower, waiting for robot to advance. */
+ CLAMP_DROPING_WAITING_ROBOT,
+ /* Clamp locked in a bay. */
+ CLAMP_LOCKED,
+ /* Clamp blocked. */
+ CLAMP_BLOCKED,
+
+ /* Waiting movement order. */
+ CLAMP_MOVE_IDLE,
/* Moving to a final or intermediary position. */
- CLAMP_ROUTING)
+ CLAMP_MOVE_ROUTING,
+ /* Moving to source slot. */
+ CLAMP_MOVE_SRC_ROUTING,
+ /* Closing the clamp once arrived at source. */
+ CLAMP_MOVE_SRC_CLAMP_CLOSING,
+ /* Opening door once clamp closed. */
+ CLAMP_MOVE_SRC_DOOR_OPENDING,
+ /* Moving to destination slot. */
+ CLAMP_MOVE_DST_ROUTING,
+ /* Closing door once arrived at destination. */
+ CLAMP_MOVE_DST_DOOR_CLOSING,
+ /* Opening the clamp once door closed. */
+ CLAMP_MOVE_DST_CLAMP_OPENING)
FSM_EVENTS (
+ /* New element inside bottom slot. */
+ clamp_new_element,
+ /* Order to prepare tower. */
+ clamp_prepare,
+ /* Sent when an element has just been taken (door closed). */
+ clamp_taken,
+ /* Sent when clamp is working. */
+ clamp_working,
+ /* Sent when clamp return to idle state. */
+ clamp_done,
+ /* Sent when clamp is blocked. */
+ clamp_blocked,
+ /* Order to drop elements. */
+ clamp_drop,
+ /* Sent once drop is done, but robot should advance to completely
+ * free the dropped tower. */
+ clamp_drop_waiting,
+ /* Received when top FSM made the robot advance after a drop. */
+ clamp_drop_clear,
/* Order to move the clamp. */
clamp_move,
+ /* Clamp movement success. */
+ clamp_move_success,
+ /* Clamp movement failure. */
+ clamp_move_failure,
/* Elevation and elevation motor success. */
clamp_elevation_rotation_success,
+ /* Elevation or elevation motor failure. */
+ clamp_elevation_or_rotation_failure,
+ /* Elevation motor success. */
+ clamp_elevation_success,
/* Elevation motor failure. */
clamp_elevation_failure,
+ /* Rotation motor success. */
+ clamp_rotation_success,
/* Rotation motor failure. */
clamp_rotation_failure)
-FSM_START_WITH (CLAMP_IDLE)
+FSM_START_WITH (CLAMP_START)
+FSM_START_WITH (CLAMP_MOVE_IDLE)
/** Clamp context. */
struct clamp_t
{
- /* Current position. */
+ /** True if clamp is working. */
+ uint8_t working;
+ /** Current position. */
uint8_t pos_current;
- /* Requested position. */
+ /** Requested position. */
uint8_t pos_request;
+ /** Element moving destination. */
+ uint8_t moving_to;
+ /** Position of a new element. */
+ uint8_t pos_new;
+ /** New element type. */
+ uint8_t new_element_type;
+ /** Drop direction, drop on the other side. */
+ uint8_t drop_direction;
+ /** True if clamp is open. */
+ uint8_t open;
+ /** True if clamp position is controled. */
+ uint8_t controled;
+ /** Contact state at start of move, for head check. */
+ uint8_t contact_head_before_move;
};
/** Global context. */
@@ -67,35 +173,185 @@ struct clamp_t clamp_global;
/** Clamp positions. */
static const uint16_t clamp_pos[][2] = {
{ BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP,
- BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP },
{ BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP,
- BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP },
{ BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP,
- BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP },
{ BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP,
- BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP },
{ BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP,
- BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP },
{ BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP,
- BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP },
{ BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP,
BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
{ BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP,
BOT_CLAMP_BAY_FRONT_ROTATION_STEP },
+ { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
{ BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP,
BOT_CLAMP_BAY_BACK_ROTATION_STEP },
+ { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP,
+ BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
{ BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP,
BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
};
+/** Slot doors. */
+static const uint8_t clamp_slot_door[] = {
+ BOT_PWM_DOOR_FRONT_BOTTOM,
+ 0xff,
+ BOT_PWM_DOOR_FRONT_TOP,
+ BOT_PWM_DOOR_BACK_BOTTOM,
+ 0xff,
+ BOT_PWM_DOOR_BACK_TOP,
+ 0xff
+};
+
+static void
+clamp_openclose (uint8_t open);
+
+static void
+clamp_route (void);
+
+static void
+clamp_head_check_prepare (uint8_t from);
+
+void
+clamp_init (void)
+{
+ ctx.open = 1;
+}
+
+uint8_t
+clamp_working (void)
+{
+ return ctx.working;
+}
+
void
clamp_move (uint8_t pos)
{
if (pos != ctx.pos_current)
{
ctx.pos_request = pos;
+ ctx.moving_to = CLAMP_POS_NB;
FSM_HANDLE (AI, clamp_move);
}
+ else
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
+}
+
+void
+clamp_move_element (uint8_t from, uint8_t to)
+{
+ assert (from != to);
+ ctx.pos_request = from;
+ ctx.moving_to = to;
+ clamp_head_check_prepare (from);
+ FSM_HANDLE (AI, clamp_move);
+}
+
+void
+clamp_new_element (uint8_t pos, uint8_t element_type)
+{
+ assert (pos == CLAMP_SLOT_FRONT_BOTTOM || pos == CLAMP_SLOT_BACK_BOTTOM);
+ ctx.pos_new = pos;
+ ctx.new_element_type = element_type;
+ FSM_HANDLE (AI, clamp_new_element);
+}
+
+void
+clamp_prepare (uint8_t prepare)
+{
+ logistic_global.prepare = prepare;
+ FSM_HANDLE (AI, clamp_prepare);
+}
+
+uint8_t
+clamp_drop (uint8_t drop_direction)
+{
+ if (FSM_CAN_HANDLE (AI, clamp_drop))
+ {
+ ctx.drop_direction = drop_direction;
+ FSM_HANDLE (AI, clamp_drop);
+ return 1;
+ }
+ else
+ return 0;
+}
+
+void
+clamp_drop_clear (void)
+{
+ FSM_HANDLE (AI, clamp_drop_clear);
+}
+
+void
+clamp_door (uint8_t pos, uint8_t open)
+{
+ if (pos == 0xff)
+ clamp_openclose (open);
+ else if (clamp_slot_door[pos] != 0xff)
+ {
+ if (open)
+ pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_OPEN (pos));
+ else
+ pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_CLOSE (pos));
+ }
+}
+
+uint8_t
+clamp_handle_event (void)
+{
+ if (FSM_CAN_HANDLE (AI, clamp_new_element))
+ {
+ uint8_t element_type;
+ element_type = pawn_sensor_get (DIRECTION_FORWARD);
+ if (element_type)
+ {
+ clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element_type);
+ return 1;
+ }
+ element_type = pawn_sensor_get (DIRECTION_BACKWARD);
+ if (element_type)
+ {
+ clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element_type);
+ return 1;
+ }
+ }
+ /* Handle special hardware offset. */
+ uint16_t rotation_position = mimot_get_motor1_position ();
+ if ((ctx.pos_current == CLAMP_BAY_FRONT_LEAVING
+ && rotation_position > (BOT_CLAMP_BAY_SIDE_ROTATION_STEP +
+ BOT_CLAMP_BAY_FRONT_ROTATION_STEP) / 2)
+ || (ctx.pos_current == CLAMP_BAY_BACK_LEAVING
+ && rotation_position < (BOT_CLAMP_BAY_BACK_ROTATION_STEP +
+ BOT_CLAMP_BAY_SIDE_ROTATION_STEP) / 2))
+ {
+ /* Go directly to next point. */
+ clamp_route ();
+ }
+ return 0;
+}
+
+/** Open or close clamp and adjust rotation. */
+static void
+clamp_openclose (uint8_t open)
+{
+ if (open)
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN);
+ else
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE);
+ if (ctx.controled)
+ {
+ int16_t offset = open ? 0
+ : BOT_CLAMP_CLOSED_ROTATION_OFFSET (ctx.pos_current);
+ mimot_move_motor1_absolute (clamp_pos[ctx.pos_current][1] + offset,
+ BOT_CLAMP_ROTATION_OFFSET_SPEED);
+ }
+ ctx.open = open;
}
/** Find next position and start motors. */
@@ -126,14 +382,30 @@ clamp_route (void)
}
else if (pos_current == CLAMP_BAY_FRONT_LEAVE)
{
- if (pos_request == CLAMP_SLOT_SIDE)
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request))
+ pos_new = pos_request;
+ else
+ pos_new = CLAMP_BAY_FRONT_LEAVING;
+ }
+ else if (pos_current == CLAMP_BAY_BACK_LEAVE)
+ {
+ if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_request))
+ pos_new = pos_request;
+ else
+ pos_new = CLAMP_BAY_BACK_LEAVING;
+ }
+ else if (pos_current == CLAMP_BAY_FRONT_LEAVING)
+ {
+ if (pos_request == CLAMP_SLOT_SIDE
+ || pos_request == CLAMP_BAY_SIDE_ENTER_LEAVE)
pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE;
else
pos_new = CLAMP_SLOT_BACK_MIDDLE;
}
- else if (pos_current == CLAMP_BAY_BACK_LEAVE)
+ else if (pos_current == CLAMP_BAY_BACK_LEAVING)
{
- if (pos_request == CLAMP_SLOT_SIDE)
+ if (pos_request == CLAMP_SLOT_SIDE
+ || pos_request == CLAMP_BAY_SIDE_ENTER_LEAVE)
pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE;
else
pos_new = CLAMP_SLOT_FRONT_MIDDLE;
@@ -151,32 +423,584 @@ clamp_route (void)
/* Run motors. */
mimot_move_motor0_absolute (clamp_pos[pos_new][0],
BOT_CLAMP_ELEVATION_SPEED);
- mimot_move_motor1_absolute (clamp_pos[pos_new][1],
+ int16_t offset = ctx.open ? 0
+ : BOT_CLAMP_CLOSED_ROTATION_OFFSET (pos_new);
+ mimot_move_motor1_absolute (clamp_pos[pos_new][1] + offset,
BOT_CLAMP_ROTATION_SPEED);
+ ctx.controled = 1;
/* Remember new position. */
ctx.pos_current = pos_new;
}
-FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING)
+static void
+clamp_taken_pawn (uint8_t element_type)
{
- clamp_route ();
- return FSM_NEXT (CLAMP_IDLE, clamp_move);
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ element_taken (element_nearest_element_id (robot_pos), element_type);
+}
+
+/* When lifting an element, we can discover it is actually a head. In this
+ * case, change destination. */
+static void
+clamp_head_check (void)
+{
+ uint8_t from = logistic_global.moving_from;
+ if (!ctx.contact_head_before_move && logistic_global.slots[from] == ELEMENT_PAWN)
+ {
+ /* Look head contact. */
+ uint8_t contact_head = 0;
+ if (from == CLAMP_SLOT_FRONT_BOTTOM)
+ contact_head = !IO_GET (CONTACT_FRONT_TOP);
+ else if (from == CLAMP_SLOT_BACK_BOTTOM)
+ contact_head = !IO_GET (CONTACT_BACK_TOP);
+ /* Change? */
+ if (contact_head)
+ {
+ logistic_element_change (from, ELEMENT_KING);
+ clamp_taken_pawn (ELEMENT_HEAD);
+ if (logistic_global.moving_from != from)
+ /* Cancel move. */
+ ctx.pos_request = ctx.moving_to = from;
+ else
+ /* Change move. */
+ ctx.pos_request = ctx.moving_to = logistic_global.moving_to;
+ }
+ }
+}
+
+/** Prepare head check, if contact is set yet, this is not a new head. */
+static void
+clamp_head_check_prepare (uint8_t from)
+{
+ ctx.contact_head_before_move = 0;
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (from))
+ ctx.contact_head_before_move = !IO_GET (CONTACT_FRONT_TOP);
+ else if (CLAMP_IS_SLOT_IN_BACK_BAY (from))
+ ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP);
+}
+
+/* When clamp moved to bottom slot, we can discover it is actually a tower.
+ * In this case, stop movement. */
+static uint8_t
+clamp_tower_check (void)
+{
+ uint8_t from = logistic_global.moving_from;
+ if ((from == CLAMP_SLOT_FRONT_BOTTOM || from == CLAMP_SLOT_BACK_BOTTOM)
+ && ctx.pos_current == from
+ && logistic_global.slots[from] == ELEMENT_PAWN)
+ {
+ /* Check for green zone. */
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ if (robot_pos.v.x < 450 || robot_pos.v.x > PG_WIDTH - 450)
+ return 0;
+ /* Look tower contact. */
+ uint8_t contact_tower;
+ if (from == CLAMP_SLOT_FRONT_BOTTOM)
+ contact_tower = !IO_GET (CONTACT_FRONT_MIDDLE);
+ else
+ contact_tower = !IO_GET (CONTACT_BACK_MIDDLE);
+ /* Change? */
+ if (contact_tower)
+ {
+ logistic_element_change (from, ELEMENT_TOWER);
+ clamp_taken_pawn (ELEMENT_TOWER);
+ return 1;
+ }
+ }
+ return 0;
+}
+
+static void
+clamp_blocked (void)
+{
+ /* Free everything. */
+ clamp_openclose (1);
+ uint16_t rotation_position = mimot_get_motor1_position ();
+ uint16_t elevation_position = mimot_get_motor0_position ();
+ if (rotation_position < BOT_CLAMP_BAY_SIDE_ROTATION_STEP
+ - BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP)
+ {
+ clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1);
+ if (elevation_position
+ > (BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP
+ + BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP) / 2)
+ {
+ clamp_door (CLAMP_SLOT_FRONT_TOP, 1);
+ logistic_dump (DIRECTION_FORWARD, 1);
+ }
+ else
+ logistic_dump (DIRECTION_FORWARD, 0);
+ }
+ else if (rotation_position > BOT_CLAMP_BAY_SIDE_ROTATION_STEP
+ + BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP)
+ {
+ clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1);
+ if (elevation_position
+ > (BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP
+ + BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP) / 2)
+ {
+ clamp_door (CLAMP_SLOT_BACK_TOP, 1);
+ logistic_dump (DIRECTION_BACKWARD, 1);
+ }
+ else
+ logistic_dump (DIRECTION_BACKWARD, 0);
+ }
+ mimot_motor0_free ();
+ mimot_motor1_free ();
+ /* Signal problem. */
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure));
+}
+
+#define CLAMP_DECISION_MOVE_ELEMENT 0
+#define CLAMP_DECISION_MOVE_TO_IDLE 1
+#define CLAMP_DECISION_CLAMP_LOCKED 2
+#define CLAMP_DECISION_DONE 3
+
+static uint8_t
+clamp_decision (uint8_t unblock)
+{
+ if (!unblock && logistic_global.moving_from != CLAMP_SLOT_NB)
+ {
+ clamp_move_element (logistic_global.moving_from,
+ logistic_global.moving_to);
+ return CLAMP_DECISION_MOVE_ELEMENT;
+ }
+ else if (logistic_global.prepare != 3
+ && (unblock
+ || logistic_global.clamp_pos_idle != ctx.pos_current))
+ {
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return CLAMP_DECISION_MOVE_TO_IDLE;
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return CLAMP_DECISION_CLAMP_LOCKED;
+ }
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return CLAMP_DECISION_DONE;
+ }
+}
+
+/* CLAMP FSM */
+
+FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING)
+{
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_INIT);
+ return FSM_NEXT (CLAMP_START, init_actuators);
}
-FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success,
- done, CLAMP_IDLE,
- next, CLAMP_ROUTING)
+FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT,
+ CLAMP_INIT_GOING_MIDDLE)
+{
+ mimot_move_motor0_absolute (mimot_get_motor0_position () +
+ BOT_CLAMP_INIT_ELEVATION_STEP,
+ BOT_CLAMP_INIT_ELEVATION_SPEED);
+ return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING);
+}
+
+FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success,
+ CLAMP_INIT_FINDING_ROTATION_EDGE)
+{
+ mimot_motor1_find_zero (BOT_CLAMP_INIT_ROTATION_SPEED, 0, 0);
+ return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success);
+}
+
+FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success,
+ CLAMP_INIT_FINDING_TOP)
+{
+ mimot_motor0_find_zero (BOT_CLAMP_INIT_ELEVATION_SPEED, 1,
+ BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP);
+ return FSM_NEXT (CLAMP_INIT_FINDING_ROTATION_EDGE,
+ clamp_rotation_success);
+}
+
+FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
+ rest, CLAMP_INIT_GOING_REST,
+ demo, CLAMP_GOING_IDLE)
+{
+ if (IO_GET (CONTACT_STRAT))
+ {
+ clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE);
+ return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
+ rest);
+ }
+ else
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
+ demo);
+ }
+}
+
+FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY)
+{
+ mimot_motor0_free ();
+ mimot_motor1_free ();
+ return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success);
+}
+
+FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE)
+{
+ pwm_set (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT_START);
+ pwm_set (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT_START);
+ pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT_START);
+ pwm_set (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT_START);
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_INIT_READY, init_start_round);
+}
+
+FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
+{
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success);
+}
+
+FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_failure, CLAMP_BLOCKED)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_failure);
+}
+
+FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
+{
+ ctx.working = 1;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_working));
+ pwm_set_timed (clamp_slot_door[ctx.pos_new],
+ BOT_PWM_DOOR_CLOSE (ctx.pos_new));
+ return FSM_NEXT (CLAMP_IDLE, clamp_new_element);
+}
+
+FSM_TRANS (CLAMP_IDLE, clamp_prepare,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ logistic_decision ();
+ switch (clamp_decision (0))
+ {
+ default:
+ case CLAMP_DECISION_MOVE_ELEMENT:
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element);
+ case CLAMP_DECISION_MOVE_TO_IDLE:
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle);
+ case CLAMP_DECISION_CLAMP_LOCKED:
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked);
+ case CLAMP_DECISION_DONE:
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, done);
+ }
+}
+
+FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
+{
+ /* If going forward, drop at back. */
+ uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
+ pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN (bay + 0));
+ pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN (bay + 2));
+ return FSM_NEXT (CLAMP_IDLE, clamp_drop);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ logistic_element_new (ctx.pos_new, ctx.new_element_type);
+ clamp_taken_pawn (ctx.new_element_type);
+ switch (clamp_decision (0))
+ {
+ default:
+ case CLAMP_DECISION_MOVE_ELEMENT:
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element);
+ case CLAMP_DECISION_MOVE_TO_IDLE:
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle);
+ case CLAMP_DECISION_CLAMP_LOCKED:
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked);
+ case CLAMP_DECISION_DONE:
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done);
+ }
+}
+
+FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ logistic_element_move_done ();
+ switch (clamp_decision (0))
+ {
+ default:
+ case CLAMP_DECISION_MOVE_ELEMENT:
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_element);
+ case CLAMP_DECISION_MOVE_TO_IDLE:
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_to_idle);
+ case CLAMP_DECISION_CLAMP_LOCKED:
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ clamp_locked);
+ case CLAMP_DECISION_DONE:
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ done);
+ }
+}
+
+FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_failure, CLAMP_BLOCKED)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
+ CLAMP_DROPING_WAITING_ROBOT)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_drop_waiting));
+ return FSM_NEXT_TIMEOUT (CLAMP_DROPING_DOOR_OPENING);
+}
+
+FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ logistic_drop (ctx.drop_direction);
+ switch (clamp_decision (0))
+ {
+ default:
+ case CLAMP_DECISION_MOVE_ELEMENT:
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ move_element);
+ case CLAMP_DECISION_MOVE_TO_IDLE:
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ move_to_idle);
+ case CLAMP_DECISION_CLAMP_LOCKED:
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ clamp_locked);
+ case CLAMP_DECISION_DONE:
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ done);
+ }
+}
+
+FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED)
+{
+ pwm_set_timed (clamp_slot_door[ctx.pos_new],
+ BOT_PWM_DOOR_CLOSE (ctx.pos_new));
+ logistic_element_new (ctx.pos_new, ctx.new_element_type);
+ clamp_taken_pawn (ctx.new_element_type);
+ return FSM_NEXT (CLAMP_LOCKED, clamp_new_element);
+}
+
+FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
+{
+ /* If going forward, drop at back. */
+ uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
+ pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN (bay + 0));
+ pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN (bay + 2));
+ return FSM_NEXT (CLAMP_LOCKED, clamp_drop);
+}
+
+FSM_TRANS (CLAMP_BLOCKED, clamp_prepare,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ switch (clamp_decision (1))
+ {
+ default:
+ case CLAMP_DECISION_MOVE_TO_IDLE:
+ return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle);
+ case CLAMP_DECISION_CLAMP_LOCKED:
+ return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked);
+ case CLAMP_DECISION_DONE:
+ return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done);
+ }
+}
+
+/* CLAMP_MOVE FSM */
+
+FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
+ move, CLAMP_MOVE_ROUTING,
+ move_element, CLAMP_MOVE_SRC_ROUTING,
+ move_element_here, CLAMP_MOVE_SRC_CLAMP_CLOSING)
+{
+ if (ctx.moving_to == CLAMP_POS_NB)
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move);
+ }
+ else
+ {
+ if (ctx.pos_current != ctx.pos_request)
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element);
+ }
+ else
+ {
+ clamp_openclose (0);
+ return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here);
+ }
+ }
+}
+
+FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
+ done, CLAMP_MOVE_IDLE,
+ next, CLAMP_MOVE_ROUTING)
{
if (ctx.pos_current == ctx.pos_request)
{
- return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
+ return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
done);
}
else
{
clamp_route ();
- return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
next);
}
}
+FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure,
+ CLAMP_MOVE_IDLE)
+{
+ clamp_blocked ();
+ return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure);
+}
+
+FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
+ cancel, CLAMP_MOVE_IDLE,
+ done, CLAMP_MOVE_SRC_CLAMP_CLOSING,
+ next, CLAMP_MOVE_SRC_ROUTING)
+{
+ if (clamp_tower_check ())
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
+ cancel);
+ }
+ else if (ctx.pos_current == ctx.pos_request)
+ {
+ clamp_openclose (0);
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
+ clamp_elevation_rotation_success, done);
+ }
+ else
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
+ clamp_elevation_rotation_success, next);
+ }
+}
+
+FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_or_rotation_failure,
+ CLAMP_MOVE_IDLE)
+{
+ clamp_blocked ();
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
+ clamp_elevation_or_rotation_failure);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME,
+ open_door, CLAMP_MOVE_SRC_DOOR_OPENDING,
+ move, CLAMP_MOVE_DST_ROUTING)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_taken));
+ if (clamp_slot_door[ctx.pos_current] != 0xff)
+ {
+ pwm_set_timed (clamp_slot_door[ctx.pos_current],
+ BOT_PWM_DOOR_OPEN (ctx.pos_current));
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door);
+ }
+ else
+ {
+ ctx.pos_request = ctx.moving_to;
+ clamp_route ();
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, move);
+ }
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING, BOT_PWM_DOOR_OPEN_TIME,
+ CLAMP_MOVE_DST_ROUTING)
+{
+ ctx.pos_request = ctx.moving_to;
+ clamp_route ();
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING);
+}
+
+FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success,
+ done_close_door, CLAMP_MOVE_DST_DOOR_CLOSING,
+ done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING,
+ next, CLAMP_MOVE_DST_ROUTING)
+{
+ clamp_head_check ();
+ if (ctx.pos_current == ctx.pos_request)
+ {
+ if (clamp_slot_door[ctx.pos_current] != 0xff)
+ {
+ pwm_set_timed (clamp_slot_door[ctx.pos_current],
+ BOT_PWM_DOOR_CLOSE (ctx.pos_current));
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_rotation_success,
+ done_close_door);
+ }
+ else
+ {
+ clamp_openclose (1);
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_rotation_success,
+ done_open_clamp);
+ }
+ }
+ else
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_rotation_success, next);
+ }
+}
+
+FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_or_rotation_failure,
+ CLAMP_MOVE_IDLE)
+{
+ clamp_blocked ();
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_or_rotation_failure);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
+ CLAMP_MOVE_DST_CLAMP_OPENING)
+{
+ clamp_openclose (1);
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
+ CLAMP_MOVE_IDLE)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING);
+}
+
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
index 104e3cb8..76dcc34b 100644
--- a/digital/io-hub/src/robospierre/clamp.h
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -36,10 +36,20 @@ enum {
CLAMP_SLOT_SIDE,
/** Leave the front bay, ready to enter side tunnel. */
CLAMP_BAY_FRONT_LEAVE,
+ /** Leaving the front bay, entered the side tunnel. When at midway point,
+ * go directly to the next position. */
+ CLAMP_BAY_FRONT_LEAVING,
/** Leave the back bay, ready to enter side tunnel. */
CLAMP_BAY_BACK_LEAVE,
- /* Enter the side bay. Position on the side, above wheels. */
+ /** Leaving the back bay, entered the side tunnel. When at midway point,
+ * go directly to the next position. */
+ CLAMP_BAY_BACK_LEAVING,
+ /** Enter the side bay. Position on the side, above wheels. */
CLAMP_BAY_SIDE_ENTER_LEAVE,
+ /** Total number of position, including intermediary positions. */
+ CLAMP_POS_NB,
+ /** Number of slots. */
+ CLAMP_SLOT_NB = CLAMP_SLOT_SIDE + 1,
};
/** Is slot in front bay? */
@@ -50,8 +60,46 @@ enum {
#define CLAMP_IS_SLOT_IN_BACK_BAY(slot) \
((slot) >= CLAMP_SLOT_BACK_BOTTOM && (slot) <= CLAMP_SLOT_BACK_TOP)
+/** Initialise clamp module. */
+void
+clamp_init (void);
+
+/** Is clamp working? */
+uint8_t
+clamp_working (void);
+
/** Move clamp to given position. */
void
clamp_move (uint8_t pos);
+/** Move element using clamp. */
+void
+clamp_move_element (uint8_t from, uint8_t to);
+
+/** Simulate the presence of a new element. */
+void
+clamp_new_element (uint8_t pos, uint8_t element_type);
+
+/** Change logisitic preparation level and update clamp state. */
+void
+clamp_prepare (uint8_t prepare);
+
+/** Drop an element tower. Return 0 if not currently possible. If
+ * drop_direction is forward, drop at the back. */
+uint8_t
+clamp_drop (uint8_t drop_direction);
+
+/** Signal robot advanced, and drop is finished. */
+void
+clamp_drop_clear (void);
+
+/** Open/close door (pos = slot) or clamp (pos = 0xff). */
+void
+clamp_door (uint8_t pos, uint8_t open);
+
+/** Examine sensors to generate new events, return non zero if an event was
+ * generated. */
+uint8_t
+clamp_handle_event (void);
+
#endif /* clamp_h */
diff --git a/digital/io-hub/src/robospierre/codebar.avr.c b/digital/io-hub/src/robospierre/codebar.avr.c
new file mode 100644
index 00000000..fcddb079
--- /dev/null
+++ b/digital/io-hub/src/robospierre/codebar.avr.c
@@ -0,0 +1,64 @@
+/* codebar.avr.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 "codebar.h"
+
+#include "defs.h"
+
+#include "modules/twi/twi.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/crc.h"
+#include "modules/utils/byte.h"
+
+#define CODEBAR_ADDRESS 0x20
+#define CODEBAR_STATUS_LENGTH 7
+
+void
+codebar_init (void)
+{
+}
+
+uint8_t
+codebar_get (uint8_t direction)
+{
+ uint8_t buffer[CODEBAR_STATUS_LENGTH];
+ /* Read status. */
+ twi_master_recv (CODEBAR_ADDRESS, buffer, sizeof (buffer));
+ uint8_t ret = twi_master_wait ();
+ if (ret != CODEBAR_STATUS_LENGTH)
+ return 0;
+ uint8_t crc = crc_compute (buffer + 1, CODEBAR_STATUS_LENGTH - 1);
+ if (crc != buffer[0])
+ return 0;
+ /* Get data. */
+ uint8_t offset = direction == DIRECTION_FORWARD ? 1 : 4;
+ uint16_t age = v8_to_v16 (buffer[offset], buffer[offset + 1]);
+ uint16_t type = buffer[offset + 2];
+ if (age > 3 * 250)
+ return 0;
+ else
+ return type;
+}
+
diff --git a/digital/io-hub/src/robospierre/codebar.h b/digital/io-hub/src/robospierre/codebar.h
new file mode 100644
index 00000000..d334f131
--- /dev/null
+++ b/digital/io-hub/src/robospierre/codebar.h
@@ -0,0 +1,36 @@
+#ifndef codebar_h
+#define codebar_h
+/* codebar.h */
+/* 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.
+ *
+ * }}} */
+
+/** Initialise module. */
+void
+codebar_init (void);
+
+/** Get element type on the specified direction. */
+uint8_t
+codebar_get (uint8_t direction);
+
+#endif /* codebar_h */
diff --git a/digital/io-hub/src/robospierre/codebar.host.c b/digital/io-hub/src/robospierre/codebar.host.c
new file mode 100644
index 00000000..68ced300
--- /dev/null
+++ b/digital/io-hub/src/robospierre/codebar.host.c
@@ -0,0 +1,56 @@
+/* codebar.host.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 "codebar.h"
+#include "defs.h"
+
+#include "modules/host/host.h"
+#include "modules/host/mex.h"
+
+uint8_t codebar_front, codebar_back;
+
+static void
+codebar_handle (void *user, mex_msg_t *msg)
+{
+ mex_msg_pop (msg, "BB", &codebar_front, &codebar_back);
+}
+
+void
+codebar_init (void)
+{
+ const char *mex_instance = host_get_instance ("io-hub0", 0);
+ uint8_t mtype = mex_node_reservef ("%s:codebar", mex_instance);
+ mex_node_register (mtype, codebar_handle, 0);
+}
+
+uint8_t
+codebar_get (uint8_t direction)
+{
+ if (direction == DIRECTION_FORWARD)
+ return codebar_front;
+ else
+ return codebar_back;
+}
+
diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h
index 55b34354..132ec050 100644
--- a/digital/io-hub/src/robospierre/contact_defs.h
+++ b/digital/io-hub/src/robospierre/contact_defs.h
@@ -25,13 +25,25 @@
*
* }}} */
-#define CONTACT_COLOR A, 7
-#define CONTACT_JACK F, 7
-#define CONTACT_EX1 E, 0
-#define CONTACT_EX2 E, 1
+#define CONTACT_COLOR E, 3
+#define CONTACT_JACK E, 5
+#define CONTACT_STRAT E, 1
+#define CONTACT_FRONT_BOTTOM A, 4
+#define CONTACT_FRONT_MIDDLE F, 4
+#define CONTACT_BACK_BOTTOM A, 5
+#define CONTACT_BACK_MIDDLE F, 5
+#define CONTACT_FRONT_TOP A, 6
+#define CONTACT_BACK_TOP F, 6
+#define CONTACT_SIDE E, 7
#define CONTACT_LIST \
- CONTACT (CONTACT_EX1) \
- CONTACT (CONTACT_EX2)
+ CONTACT (CONTACT_FRONT_BOTTOM) \
+ CONTACT (CONTACT_FRONT_MIDDLE) \
+ CONTACT (CONTACT_FRONT_TOP) \
+ CONTACT (CONTACT_BACK_BOTTOM) \
+ CONTACT (CONTACT_BACK_MIDDLE) \
+ CONTACT (CONTACT_BACK_TOP) \
+ CONTACT (CONTACT_SIDE) \
+ CONTACT (CONTACT_STRAT)
#endif /* contact_defs_h */
diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c
new file mode 100644
index 00000000..881e1be6
--- /dev/null
+++ b/digital/io-hub/src/robospierre/element.c
@@ -0,0 +1,828 @@
+/* element.c */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2011 Jérôme Jutteau
+ *
+ * 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 "element.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/geometry/distance.h"
+#include "modules/math/geometry/geometry.h"
+
+#include "chrono.h"
+#include "logistic.h"
+#include "bot.h"
+#include "playground.h"
+
+/** Offset to be used when everything fail. */
+static uint8_t failure_offset_s;
+
+/** Elements on table. */
+struct element_t element_table[] =
+{
+ /*
+ 20 elements on intersections.
+ To be symmetric, alternate % 2.
+ See ELEMENT_INTERSEC_START and ELEMENT_INTERSEC_END
+ ELEMENT_INTERSEC_END don't takes central pawn.
+ */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* top left */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, /* top right */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* middle left */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, /* middle right */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 2nd line left */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 3th line left */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 4th line left */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0, 0, 0}, /* 5th line left */
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, -30, 0, 0},
+ {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, -30, 0, 0},
+
+ /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */
+ {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 30, 0, 0},
+
+ /*
+ 10 elements on green zones.
+ To be symmetric, alternate % 2.
+ See ELEMENT_GREEN_START and ELEMENT_GREEN_END
+ */
+ {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 110, 0, 0}, /* top left */
+ {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 110, 0, 0}, /* top right */
+ {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 100, 0, 0}, /* 2nd line left */
+ {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 100, 0, 0}, /* 2nd line right */
+ {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 110, 0, 0}, /* ... */
+ {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 110, 0, 0},
+ {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 100, 0, 0},
+ {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 100, 0, 0},
+ {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100, 0, 0},
+ {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100, 0, 0},
+
+ /*
+ 36 elements in the middle of a square.
+ Altern colors in order to retrieve position % 2.
+ See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END
+ */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* Top left blue */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left red */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 10, 0}, /* bonus */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 10, 0}, /* bonus */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* 3th line left blue */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 4th line left red */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 4, 0}, /* bonus */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 4, 0}, /* bonus */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* 5th line left blue */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0},
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0},
+ {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100, 0}, /* left red */
+ {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100, 0}, /* left blue */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 9, 0}, /* middle bonus left, red. */
+ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 9, 0}, /* middle bonus right, blue. */
+ {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100, 0}, /* right red */
+ {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100, 0} /* right blue */
+};
+
+inline void
+element_set (uint8_t element_id, element_t element)
+{
+ assert (element_id < UTILS_COUNT (element_table));
+ element_table[element_id] = element;
+}
+
+void
+element_init ()
+{
+ /* Set NONE in middle of our squares and keep ELEMENT_NONE | ELEMENT_ANY
+ in others square middle. */
+ uint8_t i;
+ element_t e;
+ for (i = ELEMENT_UNLOAD_START; i <= ELEMENT_UNLOAD_END; i++)
+ {
+ e = element_get (i);
+ if ((team_color == TEAM_COLOR_LEFT &&
+ (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT &&
+ (e.attr & ELEMENT_RIGHT)))
+ {
+ e.type = ELEMENT_NONE;
+ element_set (i, e);
+ }
+ }
+
+ /* Negative bonus for the other green zone at start. */
+ /* Do not touch last green emplacement. */
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++)
+ {
+ element_t e = element_get (i);
+ if (!((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))))
+ {
+ if (e.bonus_load > 0)
+ {
+ e.bonus_load *= -1;
+ element_set (i, e);
+ }
+ }
+ }
+}
+
+int32_t
+element_unload_score (position_t robot_pos, uint8_t element_id)
+{
+ int32_t score = 0;
+ assert (element_id < UTILS_COUNT (element_table));
+ element_t e = element_get (element_id);
+
+ /* If it is not an unload zone, quit. */
+ if (!((e.attr & ELEMENT_CENTER) || (e.attr & ELEMENT_INTERSEC)))
+ return -1;
+
+ /* Unload color. */
+ if (!(
+ (e.attr & ELEMENT_CENTER) &&
+ ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT))
+ ||(team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ ))
+ return -1;
+
+ /* If there is already something here, do not score. */
+ if (e.type != ELEMENT_NONE)
+ return -1;
+
+ /* Failed element. */
+ if (e.failure_until_s
+ && e.failure_until_s + failure_offset_s < (int) (chrono_remaining_time () / 1000))
+ return -1;
+
+ /* Bonus adjust. */
+ score += e.bonus_unload * ELEMENT_BONUS_COEFF;
+
+ /* Unload distance. */
+ /* TODO: minimal distance may not be the best choice. */
+ vect_t v = e.pos;
+ int32_t dr = distance_point_point (&v, &robot_pos.v);
+ score += 4242 - dr;
+
+ /* Alignment with the robot. */
+ if (dr > 100)
+ {
+ vect_t vr = v;
+ vect_sub (&vr, &robot_pos.v);
+ vect_t u;
+ uint16_t a;
+ if (logistic_global.collect_direction == DIRECTION_FORWARD)
+ a = robot_pos.a;
+ else
+ a = robot_pos.a + G_ANGLE_UF016_DEG (180);
+ vect_from_polar_uf016 (&u, 100, a);
+ int32_t dp = vect_dot_product (&u, &vr);
+ int32_t align = dp / dr;
+ score += align;
+ }
+
+ return score;
+}
+
+uint8_t
+element_unload_best (position_t robot_pos)
+{
+ uint8_t i;
+ uint8_t best = 0xff;
+ int32_t score, best_score = 0;
+ uint8_t retry = 0;
+ do
+ {
+ if (retry && failure_offset_s != 255)
+ failure_offset_s++;
+ for (i = ELEMENT_UNLOAD_START;
+ i <= ELEMENT_UNLOAD_END;
+ i++)
+ {
+ score = element_unload_score (robot_pos , i);
+ if (best == 0xff || best_score < score)
+ {
+ best = i;
+ best_score = score;
+ }
+ }
+ } while (best_score == -1 && retry++ < 10);
+ return best;
+}
+
+int32_t
+element_score (position_t robot_pos, uint8_t element_id)
+{
+ int32_t score = 0;
+ assert (element_id < UTILS_COUNT (element_table));
+ element_t e = element_get (element_id);
+
+ if (e.attr & ELEMENT_SAFE)
+ return -1;
+
+ if ((e.attr & ELEMENT_CENTER) &&
+ ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))))
+ return -1;
+
+ if (e.type == ELEMENT_NONE &&
+ ((e.attr & ELEMENT_INTERSEC) ||(e.attr & ELEMENT_GREEN)))
+ return -1;
+
+ /* Failed element. */
+ if (e.failure_until_s
+ && e.failure_until_s + failure_offset_s < (int) (chrono_remaining_time () / 1000))
+ return -1;
+
+ if (e.type == ELEMENT_KING)
+ score += ELEMENT_KING_SCORE;
+ else if (e.type == ELEMENT_QUEEN)
+ score += ELEMENT_QUEEN_SCORE;
+ else if (e.type == ELEMENT_PAWN)
+ score += ELEMENT_PAWN;
+ else if (e.type == ELEMENT_ANY)
+ score += ELEMENT_ANY_SCORE;
+ if (e.type & ELEMENT_NONE)
+ score /= 2;
+
+ /* Add score modifier. */
+ score += e.bonus_load * ELEMENT_BONUS_COEFF;
+
+ /* We are sure of this element. */
+ if (!(e.type & ELEMENT_NONE) &&
+ (e.attr == ELEMENT_INTERSEC || e.attr == ELEMENT_CENTER) &&
+ element_id != ELEMENT_CENTRAL_PAWN)
+ score += score / 4;
+
+ /* Distance from the robot. */
+ vect_t v = e.pos;
+ int32_t dr = distance_point_point (&v, &robot_pos.v);
+ score += (4242 - dr);
+
+ /* Alignment with the robot. */
+ if (dr > 100)
+ {
+ vect_t vr = v;
+ vect_sub (&vr, &robot_pos.v);
+ vect_t u;
+ uint16_t a;
+ if (logistic_global.collect_direction == DIRECTION_FORWARD)
+ a = robot_pos.a;
+ else
+ a = robot_pos.a + G_ANGLE_UF016_DEG (180);
+ vect_from_polar_uf016 (&u, 100, a);
+ int32_t dp = vect_dot_product (&u, &vr);
+ int32_t align = dp / dr;
+ score += align;
+ }
+
+ return score;
+}
+
+uint32_t
+element_proba (uint8_t element_id)
+{
+ assert (element_id < UTILS_COUNT (element_table));
+ element_t e = element_get (element_id);
+ uint32_t p_t = chrono_remaining_time () * 500 / CHRONO_MATCH_DURATION_MS;
+ uint8_t p_pos;
+ int32_t out = 0;
+
+ /* Intersections. */
+ if (e.attr & ELEMENT_INTERSEC)
+ {
+ /* Element on our side ? */
+ if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ {
+ p_pos = 20 - element_id / 2;
+ out = p_pos * p_t;
+ }
+ /* Element on the other side ? */
+ else
+ {
+ p_pos = element_id / 2 + 1;
+ out = p_pos * p_t;
+ }
+ }
+ /* Green zone. */
+ else if (e.attr & ELEMENT_GREEN)
+ {
+ /* In our side. */
+ if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ {
+ p_pos = 5 - ((element_id - ELEMENT_GREEN_START) / 2);
+ out = p_t * p_pos;
+
+ }
+ /* Other side. */
+ else
+ {
+ p_pos = (element_id - ELEMENT_GREEN_START) / 2 + 1;
+ out = p_t * p_pos;
+ }
+
+ }
+ /* Central pawn */
+ else if (e.type == ELEMENT_CENTRAL_PAWN)
+ out = p_t;
+ /* Centre of squares. */
+ else if (e.attr & ELEMENT_CENTER)
+ {
+ /* In our side. */
+ if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ out = 0;
+ /* Other side. */
+ else
+ {
+ p_pos = (element_id - ELEMENT_UNLOAD_START) / 2 + 1;
+ out = (1000 - p_t) * p_pos;
+ }
+
+ }
+ else
+ out = 0;
+ return out;
+}
+
+uint8_t
+element_best (position_t robot_pos)
+{
+ uint8_t i;
+ uint8_t best = 0xff;
+ int32_t score = 0, best_score = 0;
+ uint8_t retry = 0;
+ do
+ {
+ if (retry && failure_offset_s != 255)
+ failure_offset_s++;
+ for (i = 0; i < UTILS_COUNT (element_table); i++)
+ {
+ score = element_score (robot_pos ,i);
+ if (best == 0xff || best_score < score)
+ {
+ best = i;
+ best_score = score;
+ }
+ }
+ } while (best_score == -1 && retry++ < 10);
+ return best;
+}
+
+void
+element_not_here (uint8_t element_id)
+{
+ assert (element_id < UTILS_COUNT (element_table));
+ element_t e = element_get (element_id);
+ e.type = ELEMENT_NONE;
+ element_set (element_id, e);
+
+ /* Invalidate the same element to the other side. */
+ if (e.attr & ELEMENT_INTERSEC)
+ {
+ uint8_t other_side_id = element_opposed (element_id);
+ element_t other_side = element_get (other_side_id);
+ other_side.type = ELEMENT_NONE;
+ element_set (other_side_id, other_side);
+ }
+
+ /* If the element is on an intersection, try to guess last elements. */
+ element_intersec_symetric (element_id, ELEMENT_NONE);
+}
+
+inline void
+element_intersec_symetric (uint8_t element_id, uint8_t element_type)
+{
+ static uint8_t element_columns[2][5] =
+ {
+ {
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN
+ },
+ {
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN,
+ ELEMENT_NONE | ELEMENT_PAWN
+ }
+ };
+ element_t e = element_get (element_id);
+
+ /* if the element is on an intersection, try to guess last elements. */
+ if ((e.attr & ELEMENT_INTERSEC) && !(e.attr & ELEMENT_CENTER))
+ {
+ uint8_t cpt_none = 0;
+ uint8_t cpt_pawn = 0;
+ uint8_t col;
+ uint8_t i;
+ uint8_t type = 0;
+
+ if (element_id % 4 <= 1)
+ col = 0;
+ else
+ col = 1;
+
+ /* Nothing to see. */
+ if (element_columns[col][4] != (ELEMENT_NONE | ELEMENT_PAWN))
+ return;
+
+ /* Count. */
+ for (i = 0; i < 5; i++)
+ {
+ if (element_columns[col][i] == ELEMENT_PAWN || element_type == ELEMENT_PAWN)
+ cpt_pawn++;
+ if (element_columns[col][i] == ELEMENT_NONE || element_type == ELEMENT_NONE)
+ cpt_none++;
+ if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN))
+ {
+ element_columns[col][i] = element_type;
+ break;
+ }
+ }
+
+ /* With which element are we going to fill the rest ? */
+ if (cpt_pawn == 2)
+ type = ELEMENT_NONE;
+ else if (cpt_none == 3)
+ type = ELEMENT_PAWN;
+ else
+ return;
+
+ for (i = 0; i < 5; i++)
+ if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN))
+ element_columns[col][i] = type;
+
+ /* Complete if possible. */
+ for (i = ELEMENT_INTERSEC_START; i < ELEMENT_INTERSEC_END; i++)
+ {
+ uint8_t col_i;
+ if (i % 4 <= 1)
+ col_i = 0;
+ else
+ col_i = 1;
+ if (col_i == col)
+ {
+ element_t el = element_get (i);
+ element_t sym = element_get (element_opposed (i));
+ if (el.type == (ELEMENT_NONE | ELEMENT_PAWN))
+ {
+ el.type = type;
+ element_set (i, el);
+ /* Set opposed. */
+ if (sym.type == (ELEMENT_NONE | ELEMENT_PAWN))
+ {
+ sym.type = type;
+ element_set (element_opposed (i), sym);
+ }
+ }
+ }
+ }
+ }
+}
+
+void
+element_taken (uint8_t element_id, uint8_t element_type)
+{
+ assert (element_id < UTILS_COUNT (element_table));
+ static uint8_t pawn_c = 3, queen_c = 1, king_c = 1, any_nb = 5;
+ uint8_t other_side_id, other_side_id_element = 0;
+
+ if (element_type != ELEMENT_PAWN && element_type != ELEMENT_QUEEN && element_type != ELEMENT_KING)
+ return;
+
+ /* Set element. */
+ element_t e = element_get (element_id);
+ e.type = ELEMENT_NONE;
+ element_set (element_id, e);
+
+ /* Deduce symmetric position. */
+ if ((e.attr & ELEMENT_INTERSEC) || (e.attr & ELEMENT_GREEN))
+ {
+ other_side_id_element = element_opposed (element_id);
+ element_t other_side = element_get (other_side_id_element);
+ if (other_side.type != ELEMENT_NONE)
+ {
+ other_side.type = element_type;
+ element_set (other_side_id_element, other_side);
+ }
+ }
+
+ /* If the element is on an intersection, try to guess last elements. */
+ element_intersec_symetric (element_id, element_type);
+
+ /* If the element is in the green zone, try to guess last elements. */
+ if ((e.attr & ELEMENT_GREEN) && any_nb > 0)
+ {
+ uint8_t i;
+ if (element_type == ELEMENT_PAWN)
+ pawn_c--;
+ else if (element_type == ELEMENT_QUEEN)
+ queen_c--;
+ else if (element_type == ELEMENT_KING)
+ king_c--;
+ any_nb--;
+
+ /* If there is something to guess. */
+ if (any_nb <= 3)
+ {
+ /* All others are pawns. */
+ if (!queen_c && !king_c)
+ {
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++)
+ {
+ element_t el = element_get (i);
+ if (el.type == ELEMENT_ANY)
+ {
+ el.type = ELEMENT_PAWN;
+ element_set (i, el);
+ any_nb--;
+ /* Set opposed side. */
+ other_side_id = element_opposed (element_id);
+ element_t other_side = element_get (other_side_id);
+ if (other_side.type != ELEMENT_NONE &&
+ other_side_id != other_side_id_element)
+ {
+ other_side.type = ELEMENT_PAWN;
+ element_set (other_side_id, other_side);
+ }
+ }
+ }
+ }
+ /* All others are unidentified heads */
+ else if (pawn_c == 0 && any_nb == 2)
+ {
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++)
+ {
+ element_t el = element_get (i);
+ if (el.type == ELEMENT_ANY)
+ {
+ el.type = ELEMENT_HEAD;
+ element_set (i, el);
+ /* Set opposed side. */
+ other_side_id = element_opposed (element_id);
+ element_t other_side = element_get (other_side_id);
+ if (other_side.type != ELEMENT_NONE &&
+ other_side_id != other_side_id_element)
+ {
+ other_side.type = ELEMENT_HEAD;
+ element_set (other_side_id, other_side);
+ }
+ }
+ }
+ }
+ /* Last element. */
+ if (any_nb == 1)
+ {
+ uint8_t last_type;
+ if (pawn_c == 1) last_type = ELEMENT_PAWN;
+ else if (queen_c == 1) last_type = ELEMENT_QUEEN;
+ else last_type = ELEMENT_KING;
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++)
+ {
+ element_t el = element_get (i);
+ if (el.type == ELEMENT_ANY || el.type == ELEMENT_HEAD)
+ {
+ el.type = last_type;
+ element_set (i, el);
+ any_nb--;
+ /* Set opposed side. */
+ other_side_id = element_opposed (i);
+ element_t other_side = element_get (other_side_id);
+ if (other_side.type != ELEMENT_NONE &&
+ other_side_id != other_side_id_element)
+ {
+ other_side.type = last_type;
+ element_set (other_side_id, other_side);
+ }
+ break;
+ }
+ }
+ }
+ }
+ }
+}
+
+void
+element_down (uint8_t element_id, uint8_t element_type)
+{
+ uint8_t i;
+ element_t e = element_get (element_id);
+ e.type = element_type;
+ element_set (element_id, e);
+
+ /* Malus elements near this element. */
+ for (i = ELEMENT_INTERSEC_START; i <= ELEMENT_INTERSEC_END; i++)
+ {
+ element_t ie = element_get (i);
+ if (UTILS_ABS (e.pos.x - ie.pos.x)
+ < BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20
+ && UTILS_ABS (e.pos.y - ie.pos.y)
+ < BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20)
+ {
+ ie.bonus_load = -50;
+ element_set (i, ie);
+ }
+ }
+}
+
+void
+element_failure (uint8_t element_id)
+{
+ element_t e = element_get (element_id);
+ e.failure_until_s = chrono_remaining_time () / 1000 - 7;
+ element_set (element_id, e);
+}
+
+uint8_t
+element_nearest_element_id (position_t robot_pos)
+{
+ uint8_t i;
+ uint8_t id = 0;
+ int32_t distance = 4242;
+ element_t e;
+ for (i = 0; i < UTILS_COUNT (element_table); i++)
+ {
+ e = element_get (i);
+ vect_t v = e.pos;
+ int32_t dr = distance_point_point (&v, &robot_pos.v);
+ if (dr < distance)
+ {
+ id = i;
+ distance = dr;
+ }
+ }
+ return id;
+}
+
+uint8_t
+element_opposed (uint8_t element_id)
+{
+ uint8_t op = 0xff;
+ element_t e = element_get (element_id);
+ if ((e.attr & ELEMENT_GREEN) ||(e.attr & ELEMENT_INTERSEC))
+ {
+ if (e.attr & ELEMENT_LEFT)
+ op = element_id + 1;
+ else
+ op = element_id - 1;
+ }
+ return op;
+}
+
+vect_t
+element_get_pos (uint8_t element_id)
+{
+ element_t e = element_get (element_id);
+ vect_t pos;
+ pos = e.pos;
+ if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT))
+ {
+ /* To the right. */
+ pos.x = PG_WIDTH - BOT_GREEN_ELEMENT_DISTANCE_MM;
+ }
+ if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT))
+ {
+ /* To the left. */
+ pos.x = BOT_GREEN_ELEMENT_DISTANCE_MM;
+ }
+ return pos;
+}
+
+uint8_t
+element_blocking (uint8_t element_id, uint8_t escape)
+{
+ element_t e = element_get (element_id);
+ return e.type == ELEMENT_TOWER || (!escape && e.type == ELEMENT_PAWN);
+}
+
+uint8_t
+element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape)
+{
+ uint8_t i;
+ element_t e;
+ /* Only unload area are blocking. */
+ for (i = ELEMENT_UNLOAD_START; i <= ELEMENT_UNLOAD_END; i++)
+ {
+ e = element_get (i);
+ if (e.type == ELEMENT_TOWER || (!escape && e.type == ELEMENT_PAWN))
+ {
+ /* Compute square of distance to obstacle, see
+ * distance_segment_point in modules/math/geometry for the method
+ * explanation. */
+ int32_t absq = (int32_t) ab * ab;
+ vect_t vab = b; vect_sub (&vab, &a);
+ vect_t vao = e.pos; vect_sub (&vao, &a);
+ int32_t dp = vect_dot_product (&vab, &vao);
+ int32_t dsq;
+ if (dp < 0)
+ {
+ dsq = vect_dot_product (&vao, &vao);
+ }
+ else if (dp > absq)
+ {
+ vect_t vbo = e.pos; vect_sub (&vbo, &b);
+ dsq = vect_dot_product (&vbo, &vbo);
+ }
+ else
+ {
+ vect_t vabn = vab; vect_normal (&vabn);
+ dsq = vect_dot_product (&vabn, &vao) / ab;
+ dsq *= dsq;
+ }
+ /* Compare with square of authorised distance. */
+ if (dsq < (int32_t) (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) *
+ (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20))
+ return 1;
+ }
+ }
+ return 0;
+}
+
+void
+element_i_like_green ()
+{
+ /* Negative bonus for the other green zone at start. */
+ /* Do not touch last green emplacement. */
+ int i;
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++)
+ {
+ element_t e = element_get (i);
+ if (!((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ && e.bonus_load < 0
+ && (e.type == ELEMENT_KING || e.type == ELEMENT_QUEEN))
+ {
+ e.bonus_load = 40;
+ element_set (i, e);
+ }
+ }
+}
+
+void
+element_no_more_green (void)
+{
+ uint8_t i;
+ /* Remove our green zone score at first unload. */
+ for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++)
+ {
+ element_t e = element_get (i);
+ if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) ||
+ (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))
+ {
+ e.bonus_load = 0;
+ element_set (i, e);
+ }
+ }
+}
diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h
new file mode 100644
index 00000000..41985e95
--- /dev/null
+++ b/digital/io-hub/src/robospierre/element.h
@@ -0,0 +1,213 @@
+#ifndef element_h
+#define element_h
+/* element.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 Nicolas Schodet, Jérôme Jutteau
+ *
+ * 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"
+
+/** None. */
+#define ELEMENT_NONE 1
+/** A pawn, not a head. */
+#define ELEMENT_PAWN 2
+/** Queen pawn, used for statistic update of table data. */
+#define ELEMENT_QUEEN 4
+/** King pawn, used for statistic update of table data. */
+#define ELEMENT_KING 8
+/** Any head, queen or king. */
+#define ELEMENT_HEAD (ELEMENT_QUEEN |ELEMENT_KING)
+/** Any element, pawn, queen, or king. */
+#define ELEMENT_ANY (ELEMENT_HEAD |ELEMENT_PAWN)
+/** Tower types. */
+#define ELEMENT_TOWER_1_QUEEN 16
+#define ELEMENT_TOWER_2_QUEEN 32
+#define ELEMENT_TOWER_1_KING 64
+#define ELEMENT_TOWER_2_KING 128
+
+#define ELEMENT_TOWER (ELEMENT_TOWER_1_QUEEN | \
+ ELEMENT_TOWER_2_QUEEN | \
+ ELEMENT_TOWER_1_KING | \
+ ELEMENT_TOWER_2_KING)
+
+/** Return non zero if element is a head, not a pawn. */
+#define ELEMENT_IS_HEAD(e) ((e) && !((e) & ELEMENT_PAWN))
+
+/** Return non zero if element may be a head. */
+#define ELEMENT_CAN_BE_HEAD(e) ((e) & ELEMENT_HEAD)
+
+#define ELEMENT_PAWN_SCORE 10
+#define ELEMENT_ANY_SCORE 15
+#define ELEMENT_QUEEN_SCORE 20 * 30
+#define ELEMENT_KING_SCORE 30 * 30
+
+/** Emplacement attributes bits. */
+#define ELEMENT_BONUS 1
+/**
+ * Two meanings:
+ * - When it is on an intersection or in the green zone, this mean that it is positioned on the
+ * left size of the table.
+ * - When it is located on an color, this corresponds to the red color.
+ */
+#define ELEMENT_LEFT 2
+/**
+ * Two meanings:
+ * - When it is on an intersection or in the green zone, this mean that it is positioned on the
+ * left right of the table.
+ * - When it is located on an color, this corresponds to the blue color.
+ */
+#define ELEMENT_RIGHT 4
+#define ELEMENT_SAFE 8
+#define ELEMENT_GREEN 16
+#define ELEMENT_INTERSEC 64
+/** Center of a square or central element. */
+#define ELEMENT_CENTER 128
+
+struct element_t
+{
+ /** May have several types if unknown (pawn, king, queen). */
+ uint8_t type;
+ /** Element position. */
+ vect_t pos;
+ /** Emplacement attributes. */
+ uint8_t attr;
+ /** Bonus coefficient (or Mallus if negative) for load scores. */
+ int8_t bonus_load;
+ /** Bonus coefficient (or Mallus if negative) for unload scores. */
+ int8_t bonus_unload;
+ /** Failure expiration date, can not take this element until chrono is
+ * lower than this date in seconds. */
+ int8_t failure_until_s;
+
+};
+typedef struct element_t element_t;
+
+/* Elements range, see element table content. */
+#define ELEMENT_INTERSEC_START 0
+#define ELEMENT_INTERSEC_END 19
+#define ELEMENT_CENTRAL_PAWN 20
+#define ELEMENT_GREEN_START 21
+#define ELEMENT_GREEN_END 30
+#define ELEMENT_UNLOAD_START 31
+#define ELEMENT_UNLOAD_END 66
+
+#define ELEMENT_BONUS_COEFF 100
+
+/** Elements on table. */
+extern struct element_t element_table[];
+
+/** Initialize elements. */
+void
+element_init (void);
+
+/** Gives the score of an element considering it as an unload zone. */
+int32_t
+element_unload_score (position_t robot_pos, uint8_t element_id);
+
+/** Gives best unload. */
+uint8_t
+element_unload_best (position_t robot_pos);
+
+/** Gives score of an element. */
+int32_t
+element_score (position_t robot_pos, uint8_t element_id);
+
+/** Return a probability of an element to be here.
+ * This depends of the remaining time.
+ * Returns a probability from 0 (element may really not be here)
+ * to maximal value (element may be here).
+ */
+uint32_t
+element_proba (uint8_t element_id);
+
+/** Return the best element to pick. */
+uint8_t
+element_best (position_t robot_pos);
+
+/** Function to call when we see an element is not here. */
+void
+element_not_here (uint8_t element_id);
+
+/** Call this function when an element is taken. */
+void
+element_taken (uint8_t element_id, uint8_t element_type);
+
+/** Call this function when the robot put down an element. */
+void
+element_down (uint8_t element_id, uint8_t element_type);
+
+/** Call this function when taking an element failled. */
+void
+element_failure (uint8_t element_id);
+
+/** Gives the nearest element from a position. */
+uint8_t
+element_give_position (position_t pos);
+
+/** Give the opposed element. */
+uint8_t
+element_opposed (uint8_t element_id);
+
+/** Return 1 if the element is in our side, 0 otherwise. */
+int
+element_our_side (uint8_t element_id);
+
+inline void
+element_intersec_symetric (uint8_t element_id, uint8_t element_type);
+
+uint8_t
+element_nearest_element_id (position_t robot_pos);
+
+/**
+ Give the position where the robot need to be placed to get an element.
+ */
+vect_t
+element_get_pos (uint8_t element_id);
+
+extern inline element_t
+element_get (uint8_t element_id)
+{
+ return element_table[element_id];
+}
+
+/** Return whether an element is blocking robot. */
+uint8_t
+element_blocking (uint8_t element_id, uint8_t blocking);
+
+/** Return whether an element is blocking a line segment.
+ * - a: line segment first point.
+ * - b: line segment second point.
+ * - ab: line segment length.
+ * - escape: trying to escape, be a little bit more permissive.
+ * - returns: 1 if the path should not be used. */
+uint8_t
+element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape);
+
+/** Ask to adjust score for the opposed green zone. */
+void
+element_i_like_green ();
+
+/** Do not go to our green zone any more. */
+void
+element_no_more_green (void);
+
+#endif /* element_h */
diff --git a/digital/io-hub/src/robospierre/init_defs.h b/digital/io-hub/src/robospierre/init_defs.h
new file mode 100644
index 00000000..6ce6eaae
--- /dev/null
+++ b/digital/io-hub/src/robospierre/init_defs.h
@@ -0,0 +1,50 @@
+#ifndef init_defs_h
+#define init_defs_h
+/* init_defs.h */
+/* 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 "playground.h"
+#include "bot.h"
+
+/** Parameters to push the first wall. */
+#define INIT_FIRST_WALL_PUSH \
+ 0, PG_X (BOT_FRONT_CONTACT_DIST_MM), 200, \
+ PG_A_DEG (180 + BOT_FRONT_CONTACT_ANGLE_ERROR_DEG)
+/** Parameters to go away from the first wall. */
+#define INIT_FIRST_WALL_AWAY -500
+/** Parameter to face the second wall. */
+#define INIT_SECOND_WALL_ANGLE PG_A_DEG (90)
+/** Parameters to push the second wall. */
+#define INIT_SECOND_WALL_PUSH \
+ 0, -1, PG_Y (PG_LENGTH - BOT_FRONT_CONTACT_DIST_MM), -1
+/** Parameters to go away from the second wall. */
+#define INIT_SECOND_WALL_AWAY -(200 - BOT_FRONT_CONTACT_DIST_MM)
+/** Parameter to face the start position. */
+#define INIT_START_POSITION_ANGLE PG_A_DEG (0)
+/** Start position. */
+#define INIT_START_POSITION \
+ PG_X (200), PG_Y (PG_LENGTH - 200), PG_A_DEG (0), ASSERV_BACKWARD
+
+#endif /* init_defs_h */
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
new file mode 100644
index 00000000..82f5a4fa
--- /dev/null
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -0,0 +1,622 @@
+/* logistic.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 "logistic.h"
+
+#include "clamp.h"
+#include "defs.h"
+
+#include "contact.h"
+#include "io.h"
+
+#include "debug.host.h"
+
+/** Handle elements stored inside the robot. */
+
+/** Global context. */
+struct logistic_t logistic_global;
+#define ctx logistic_global
+
+inline void
+logistic_debug_dump (void)
+{
+#ifdef HOST
+ uint8_t i;
+ static const char *names[][CLAMP_SLOT_NB] = {
+ { "f1", "f2", "f3", "b1", "b2", "b3", "s1" },
+ { "F1", "F2", "F3", "B1", "B2", "B3", "S1" }
+ };
+ static const char *names_dir[] = { "--", "<-", "->" };
+ DPRINTF ("%s", names_dir[ctx.collect_direction]);
+ for (i = 0; i < CLAMP_SLOT_NB; i++)
+ {
+ DPRINTF (" %s", ctx.slots[i]
+ ? names[ELEMENT_IS_HEAD (ctx.slots[i]) ? 1 : 0][i]
+ : "__");
+ }
+ if (ctx.moving_from != CLAMP_SLOT_NB)
+ {
+ DPRINTF (" %s => %s", names[0][ctx.moving_from], names[0][ctx.moving_to]);
+ }
+ DPRINTF ("\n");
+ DPRINTF ("construct_possible: %u\n", ctx.construct_possible);
+ DPRINTF ("prepare: %u\n", ctx.prepare);
+ DPRINTF ("ready: %u\n", ctx.ready);
+ DPRINTF ("need_prepare: %u\n", ctx.need_prepare);
+#endif
+}
+
+/** Return 1 if location corresponds to element. */
+inline uint8_t
+logistic_case_test (uint8_t loc, uint8_t e)
+{
+ if (e == LOG_a && ctx.slots[loc])
+ return 1;
+ if (e == LOG__)
+ return 1;
+ if ((e == LOG_e || e == LOG_D) && !ctx.slots[loc])
+ return 1;
+ if ((e == LOG_P || e == LOG_p) && ctx.slots[loc] &&
+ !ELEMENT_IS_HEAD (ctx.slots[loc]))
+ return 1;
+ if ((e == LOG_H || e == LOG_h) && ELEMENT_IS_HEAD (ctx.slots[loc]))
+ return 1;
+ if (e == LOG_t && ctx.slots[loc] == ELEMENT_TOWER)
+ return 1;
+ return 0;
+}
+
+static uint8_t
+logistic_case (uint8_t e1, uint8_t e2, uint8_t e3, uint8_t e4, uint8_t e5,
+ uint8_t e6, uint8_t e7, uint8_t new_dir, uint8_t ready,
+ uint8_t check_symetric)
+{
+ /* Define direction bay and opposed bay. */
+ uint8_t dir_bay, opp_bay;
+ if (ctx.collect_direction == DIRECTION_FORWARD)
+ {
+ dir_bay = CLAMP_SLOT_FRONT_BOTTOM;
+ opp_bay = CLAMP_SLOT_BACK_BOTTOM;
+ }
+ else
+ {
+ dir_bay = CLAMP_SLOT_BACK_BOTTOM;
+ opp_bay = CLAMP_SLOT_FRONT_BOTTOM;
+ }
+ /*if (check_symetric)
+ {
+ uint8_t tmp = dir_bay;
+ dir_bay = opp_bay;
+ opp_bay = tmp;
+ }*/
+
+ /* Emplacement of elements. */
+ uint8_t
+ e1_loc = dir_bay + 2,
+ e3_loc = dir_bay + 1,
+ e6_loc = dir_bay,
+ e4_loc = CLAMP_SLOT_SIDE,
+ e2_loc = opp_bay + 2,
+ e5_loc = opp_bay + 1,
+ e7_loc = opp_bay;
+
+ /* Check elements are here. */
+ if (!(logistic_case_test (e1_loc, e1) &&
+ logistic_case_test (e2_loc, e2) &&
+ logistic_case_test (e3_loc, e3) &&
+ logistic_case_test (e4_loc, e4) &&
+ logistic_case_test (e5_loc, e5) &&
+ logistic_case_test (e6_loc, e6) &&
+ logistic_case_test (e7_loc, e7)))
+ {
+ /* Check mirror. */
+ /*if (!check_symetric)
+ logistic_case (e1, e2, e3, e4, e5, e6, e7, new_dir, ready, 1);*/
+ return 0;
+ }
+
+ /* Find source/destination if we have to make a move. */
+ /* Find source. */
+ uint8_t src = CLAMP_SLOT_NB, dst = CLAMP_SLOT_NB;
+ if (e1 == LOG_P ||e1 == LOG_H)
+ src = e1_loc;
+ else if (e3 == LOG_P ||e3 == LOG_H)
+ src = e3_loc;
+ else if (e6 == LOG_P ||e6 == LOG_H)
+ src = e6_loc;
+ else if (e4 == LOG_P ||e4 == LOG_H)
+ src = e4_loc;
+ else if (e2 == LOG_P ||e2 == LOG_H)
+ src = e2_loc;
+ else if (e5 == LOG_P ||e5 == LOG_H)
+ src = e5_loc;
+ else if (e7 == LOG_P ||e7 == LOG_H)
+ src = e7_loc;
+
+ /* Find destination. */
+ if (e1 == LOG_D)
+ dst = e1_loc;
+ else if (e3 == LOG_D)
+ dst = e3_loc;
+ else if (e6 == LOG_D)
+ dst = e6_loc;
+ else if (e4 == LOG_D)
+ dst = e4_loc;
+ else if (e2 == LOG_D)
+ dst = e2_loc;
+ else if (e5 == LOG_D)
+ dst = e5_loc;
+ else if (e7 == LOG_D)
+ dst = e7_loc;
+
+ /* We are making a move. */
+ if (src != CLAMP_SLOT_NB && dst != CLAMP_SLOT_NB)
+ {
+ if (ctx.slots[src] && !ctx.slots[dst])
+ {
+ ctx.moving_from = src;
+ ctx.moving_to = dst;
+ ctx.ready = 0;
+ }
+ }
+
+ /* Set collect direction. */
+ /* LEFT means we keep the same side, RIGHT means we put the opposed side. */
+ if (new_dir == LOG_DIR_RIGHT)
+ {
+ if (ctx.collect_direction == DIRECTION_FORWARD)
+ ctx.collect_direction = DIRECTION_BACKWARD;
+ else
+ ctx.collect_direction = DIRECTION_FORWARD;
+ }
+
+ /* Don't touch clamp's idle position if broken. */
+ if (ctx.prepare != 3)
+ {
+ /* Set clamp position idle. */
+ if (ctx.collect_direction == DIRECTION_FORWARD)
+ ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE;
+ else
+ ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE;
+ }
+ /* Set ready */
+ ctx.ready = ready;
+ return 1;
+}
+
+static void
+logistic_update_construct_possible ()
+{
+ uint8_t pawn = 0, head = 0, i;
+ /* Check for tower. */
+ if (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM] == ELEMENT_TOWER ||
+ ctx.slots[CLAMP_SLOT_BACK_BOTTOM] == ELEMENT_TOWER)
+ {
+ ctx.construct_possible = 1;
+ return;
+ }
+
+ for (i = CLAMP_SLOT_FRONT_BOTTOM; i < CLAMP_SLOT_NB; i++)
+ {
+ if (ELEMENT_IS_HEAD (ctx.slots[i]))
+ head++;
+ else if (ctx.slots[i])
+ pawn++;
+ if (head >= 1 && pawn >= 2)
+ {
+ ctx.construct_possible = 1;
+ return;
+ }
+ }
+ if (pawn || head)
+ ctx.construct_possible = 2;
+ else
+ ctx.construct_possible = 0;
+}
+
+static void
+logistic_update_need_prepare ()
+{
+ uint8_t i, head = 0, pawn = 0;
+ for (i = CLAMP_SLOT_FRONT_BOTTOM; i < CLAMP_SLOT_NB; i++)
+ {
+ if (ELEMENT_IS_HEAD (ctx.slots[i]))
+ head++;
+ else if (ctx.slots[i])
+ pawn++;
+ }
+ if ((head == 1 && pawn == 3) ||
+ head == 2 || pawn >= 4)
+ ctx.need_prepare = 1;
+ else
+ ctx.need_prepare = 0;
+
+ /* Define direction bay and opposed bay. */
+ uint8_t dir_bay, opp_bay;
+ if (ctx.collect_direction == DIRECTION_FORWARD)
+ {
+ dir_bay = CLAMP_SLOT_FRONT_BOTTOM;
+ opp_bay = CLAMP_SLOT_BACK_BOTTOM;
+ }
+ else
+ {
+ dir_bay = CLAMP_SLOT_BACK_BOTTOM;
+ opp_bay = CLAMP_SLOT_FRONT_BOTTOM;
+ }
+ /* If a head appear at the back (?) */
+ if (ELEMENT_IS_HEAD (ctx.slots[opp_bay]))
+ ctx.need_prepare = 1;
+
+ /* We founded a tower ! */
+ if (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM] == ELEMENT_TOWER ||
+ ctx.slots[CLAMP_SLOT_FRONT_BOTTOM] == ELEMENT_TOWER)
+ ctx.need_prepare = 1;
+}
+
+static void
+logisitic_make_broken ()
+{
+ LOGISTIC_CASE (_, _,
+ _, _, _,
+ a, _, RIGHT, 1);
+
+ LOGISTIC_CASE (_, _,
+ _, _, _,
+ _, a, LEFT, 1);
+}
+
+static void
+logistic_make_tower ()
+{
+ LOGISTIC_CASE (_, _,
+ _, _, _,
+ _, t, LEFT, 1);
+
+ LOGISTIC_CASE (_, _,
+ _, _, _,
+ t, _, RIGHT, 1);
+
+ LOGISTIC_CASE (D, _,
+ e, _, _,
+ H, _, RIGHT, 0);
+
+ LOGISTIC_CASE (_, h,
+ _, _, p,
+ _, p, LEFT, 1);
+
+ LOGISTIC_CASE (P, h,
+ e, _, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (_, h,
+ e, _, e,
+ P, D, LEFT, 0);
+
+ LOGISTIC_CASE (_, h,
+ _, P, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (P, h,
+ e, _, D,
+ _, p, LEFT, 0);
+
+ LOGISTIC_CASE (_, h,
+ e, _, D,
+ P, p, LEFT, 0);
+
+ LOGISTIC_CASE (_, h,
+ _, P, D,
+ _, p, LEFT, 0);
+}
+
+static void
+logistic_make_unload ()
+{
+ /* Making a small tower. Move head when having pawn. */
+ LOGISTIC_CASE (D, _,
+ e, p, _,
+ H, _, RIGHT, 0);
+
+ LOGISTIC_CASE (D, p,
+ e, e, e,
+ H, _, RIGHT, 0);
+
+ LOGISTIC_CASE (D, _,
+ e, e, e,
+ H, p, RIGHT, 0);
+
+ /* Making a small tower. Move pawn under head. */
+ LOGISTIC_CASE (_, h,
+ _, P, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (P, h,
+ e, e, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (_, h,
+ e, e, e,
+ P, D, LEFT, 0);
+
+ /* Making a small tower. Finally move head on pawn. */
+ LOGISTIC_CASE (_, H,
+ _, _, D,
+ _, p, LEFT, 0);
+
+ LOGISTIC_CASE (_, _,
+ _, _, h,
+ _, p, LEFT, 1);
+
+ /* Having any element. */
+ LOGISTIC_CASE (_, _,
+ _, _, _,
+ _, a, LEFT, 1);
+
+ LOGISTIC_CASE (_, _,
+ _, _, _,
+ a, _, RIGHT, 1);
+
+ LOGISTIC_CASE (_, a,
+ _, _, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (a, _,
+ e, _, _,
+ D, _, RIGHT, 0);
+
+ LOGISTIC_CASE (_, _,
+ _, P, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (_, _,
+ e, P, _,
+ D, _, RIGHT, 0);
+}
+
+static void
+logisitic_make_switches ()
+{
+ LOGISTIC_CASE (_, P,
+ _, D, e,
+ _, _, LEFT, 0);
+
+ LOGISTIC_CASE (P, _,
+ e, D, _,
+ _, _, LEFT, 0);
+
+ LOGISTIC_CASE (_, _,
+ e, D, _,
+ P, _, LEFT, 0);
+
+ LOGISTIC_CASE (_, _,
+ _, D, e,
+ _, P, LEFT, 0);
+
+ LOGISTIC_CASE (_, D,
+ e, p, e,
+ P, _, LEFT, 0);
+
+ LOGISTIC_CASE (_, a,
+ e, p, e,
+ P, D, LEFT, 0);
+
+ LOGISTIC_CASE (D, _,
+ e, _, _,
+ H, _, RIGHT, 0);
+
+ LOGISTIC_CASE (h, P,
+ e, _, e,
+ D, _, RIGHT, 0);
+
+ LOGISTIC_CASE (h, e,
+ e, _, e,
+ D, P, RIGHT, 0);
+
+ LOGISTIC_CASE (P, h,
+ e, _, e,
+ _, D, LEFT, 0);
+
+ LOGISTIC_CASE (e, h,
+ e, _, e,
+ P, D, LEFT, 0);
+}
+
+void
+logistic_decision (void)
+{
+ /* Reset. */
+ ctx.moving_from = CLAMP_SLOT_NB;
+ ctx.moving_to = CLAMP_SLOT_NB;
+ ctx.construct_possible = 0;
+ ctx.ready = 0;
+ ctx.need_prepare = 0;
+
+ /* Update context. */
+ logistic_update_construct_possible ();
+ /* Update if a something is possible. */
+ logistic_update_need_prepare ();
+
+ /* Broken clamp. */
+ if (ctx.construct_possible && ctx.prepare == 3)
+ {
+ DPRINTF ("\nlogisitic_make_broken\n");
+ logisitic_make_broken ();
+ }
+ /* Prepare tower. */
+ else if (ctx.construct_possible == 1 && ctx.prepare != 0)
+ {
+
+ DPRINTF ("\nlogisitic_make_tower\n");
+ logistic_make_tower ();
+ }
+ /* Need to unload. */
+ else if (ctx.construct_possible == 2 && ctx.prepare == 2)
+ {
+ DPRINTF ("\nlogisitic_make_unload\n");
+ logistic_make_unload ();
+ }
+ /* Internal switches. */
+ else
+ {
+ DPRINTF ("\nlogisitic_make_switches\n");
+ logisitic_make_switches ();
+ }
+
+ logistic_debug_dump ();
+ return;
+}
+
+void
+logistic_init (void)
+{
+ uint8_t i;
+ for (i = 0; i < CLAMP_SLOT_NB; i++)
+ ctx.slots[i] = 0;
+ ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB;
+ ctx.collect_direction = DIRECTION_FORWARD;
+ ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE;
+ ctx.construct_possible = 0;
+ ctx.prepare = 1;
+ ctx.ready = 0;
+ ctx.need_prepare = 0;
+}
+
+void
+logistic_update (void)
+{
+ uint8_t side_now = !IO_GET (CONTACT_SIDE);
+ /* Filter side contact. */
+ if (side_now)
+ {
+ ctx.side_filter = 0;
+ ctx.side_state = 1;
+ }
+ else if (ctx.side_filter++ == 2 * 250)
+ {
+ ctx.side_state = 0;
+ ctx.side_filter = 0;
+ }
+ /* Side slot element can be lost. */
+ if (ctx.moving_from != CLAMP_SLOT_SIDE && !ctx.side_state)
+ ctx.slots[CLAMP_SLOT_SIDE] = 0;
+}
+
+void
+logistic_element_new (uint8_t pos, uint8_t element_type)
+{
+ assert (pos < CLAMP_SLOT_NB);
+ assert (!ctx.slots[pos]);
+ ctx.slots[pos] = element_type;
+ logistic_decision ();
+}
+
+void
+logistic_element_change (uint8_t pos, uint8_t element_type)
+{
+ assert (pos < CLAMP_SLOT_NB);
+ ctx.slots[pos] = element_type;
+ logistic_decision ();
+}
+
+void
+logistic_element_move_done (void)
+{
+ assert (!ctx.slots[ctx.moving_to] || ctx.moving_to == ctx.moving_from);
+ ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from];
+ ctx.slots[ctx.moving_from] = 0;
+ ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB;
+ logistic_decision ();
+}
+
+void
+logistic_drop (uint8_t direction)
+{
+ uint8_t bay = direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
+ uint8_t i;
+ for (i = bay; i < bay + 3; i++)
+ ctx.slots[i] = 0;
+ logistic_decision ();
+}
+
+uint8_t
+logistic_drop_element_type (uint8_t direction)
+{
+ uint8_t bay = direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
+ uint8_t nb = 0;
+ uint8_t element_type = ELEMENT_NONE;
+ uint8_t i;
+ for (i = bay; i < bay + 3; i++)
+ {
+ if (ctx.slots[i])
+ {
+ nb++;
+ element_type = ctx.slots[i];
+ }
+ }
+ return nb > 1 ? ELEMENT_TOWER : element_type;
+}
+
+void
+logistic_dump (uint8_t direction, uint8_t drop_top)
+{
+ /* Drop. */
+ uint8_t i;
+ uint8_t bay = direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_FRONT_BOTTOM : CLAMP_SLOT_BACK_BOTTOM;
+ for (i = bay; i < bay + 2 + drop_top; i++)
+ ctx.slots[i] = 0;
+}
+
+static uint8_t
+logistic_slot_clear (uint8_t slot)
+{
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot))
+ {
+ if (ctx.slots[CLAMP_SLOT_FRONT_MIDDLE])
+ return 0;
+ uint8_t middle_type = ctx.slots[CLAMP_SLOT_FRONT_BOTTOM];
+ if (ELEMENT_IS_HEAD (middle_type) || middle_type == ELEMENT_TOWER)
+ return 0;
+ }
+ else if (CLAMP_IS_SLOT_IN_BACK_BAY (slot))
+ {
+ if (ctx.slots[CLAMP_SLOT_BACK_MIDDLE])
+ return 0;
+ uint8_t middle_type = ctx.slots[CLAMP_SLOT_BACK_BOTTOM];
+ if (ELEMENT_IS_HEAD (middle_type) || middle_type == ELEMENT_TOWER)
+ return 0;
+ }
+ return 1;
+}
+
+uint8_t
+logistic_path_clear (uint8_t slot1, uint8_t slot2)
+{
+ return logistic_slot_clear (slot1) && logistic_slot_clear (slot2);
+}
+
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
new file mode 100644
index 00000000..c6bc0210
--- /dev/null
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -0,0 +1,143 @@
+#ifndef logistic_h
+#define logistic_h
+/* logistic.h */
+/* 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 "element.h"
+#include "clamp.h"
+
+/** Defines for logistic macro, see LOGISTIC_CASE. */
+/** Any element must be present (not empty). */
+#define LOG_a 0
+/** Do not care if there is an element or not. */
+#define LOG__ 1
+/** There must not be element here. */
+#define LOG_e 2
+/** Pawn to pick up. */
+#define LOG_P 4
+/** A head to pick up. */
+#define LOG_H 5
+/** Pawn who has to be present. */
+#define LOG_p 6
+/** Head who has to be present. */
+#define LOG_h 7
+/** Destination (who has to be empty). */
+#define LOG_D 8
+/** We have a tower (always at bottom). */
+#define LOG_t 9
+/* LEFT means we keep the same side, RIGHT means we put the opposed side. */
+#define LOG_DIR_LEFT 0
+#define LOG_DIR_RIGHT 1
+/** Logistic macro to test if a case correspond and to perform movement.
+ * We have to consider that the moving direction is on the left.
+ * New direction is indicated (same or opposed).
+ * Set ready or not. */
+#define LOGISTIC_CASE(e1, e2, \
+ e3, e4, e5, \
+ e6, e7, new_dir, ready) \
+{ \
+ if (logistic_case (LOG_##e1, LOG_##e2, LOG_##e3, LOG_##e4, LOG_##e5, \
+ LOG_##e6, LOG_##e7, LOG_DIR_##new_dir, ready, 0)) \
+ return; \
+}
+
+/** Logistic context. */
+struct logistic_t
+{
+ /** Current robot content.
+ *
+ * Elements are fully specified (pawn, queen or king). An exception can
+ * occurs when a codebar is read but does not correspond to a valid word.
+ *
+ * When a movement is outgoing, the element is kept in the source slot. */
+ uint8_t slots[CLAMP_SLOT_NB];
+ /** Current element movement source and destination, CLAMP_SLOT_NB if no
+ * current movement. */
+ uint8_t moving_from, moving_to;
+ /** Best collect direction. */
+ uint8_t collect_direction;
+ /** Idle clamp position, depend on collect direction. */
+ uint8_t clamp_pos_idle;
+ /** Inform TOP if no construct is possible (0), if a tower is possible (1), if an
+ * other element can be put even if the clamp is broken (2). */
+ uint8_t construct_possible;
+ /** TOP set 0 if we do not want to build something, 1 if we can build a tower, 2 to prepare
+ anything possible, 3 the clamp is broken, prepare anything with *_BOTTOM. */
+ uint8_t prepare;
+ /** Inform TOP if a construct is ready (1) or not (0). */
+ uint8_t ready;
+ /* Inform TOP that we can't take any more elements and needs to put
+ * construction somewhere. */
+ uint8_t need_prepare;
+ /** Filtered side slot sensor. */
+ uint8_t side_state;
+ /** Filtered side slot sensor counter. */
+ uint16_t side_filter;
+};
+
+/** Global context. */
+extern struct logistic_t logistic_global;
+
+/** Initialise module. */
+void
+logistic_init (void);
+
+/** Examine current state and take a decision. */
+void
+logistic_decision (void);
+
+/** To be called at regular interval to check for bad robot state. */
+void
+logistic_update (void);
+
+/** To be called when a new element is entering the robot. */
+void
+logistic_element_new (uint8_t pos, uint8_t element_type);
+
+/** Oh la la, the pawn was not a pawn, it's a head. */
+void
+logistic_element_change (uint8_t pos, uint8_t element_type);
+
+/** To be called when a element movement is done. */
+void
+logistic_element_move_done (void);
+
+/** To be called when elements have been dropped on the opposite side of
+ * direction. */
+void
+logistic_drop (uint8_t direction);
+
+/** Get element type to be dropped. */
+uint8_t
+logistic_drop_element_type (uint8_t direction);
+
+/** Dump every element on one direction. */
+void
+logistic_dump (uint8_t direction, uint8_t drop_top);
+
+/** Is path clear between two positions? */
+uint8_t
+logistic_path_clear (uint8_t slot1, uint8_t slot2);
+
+#endif /* logistic_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 3a32a1ae..9ae644e6 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -28,6 +28,8 @@
#include "modules/uart/uart.h"
#include "modules/proto/proto.h"
+#include "modules/devices/usdist/usdist.h"
+
#include "timer.h"
#include "chrono.h"
#include "simu.host.h"
@@ -38,30 +40,60 @@
#include "pwm.h"
#include "contact.h"
+#include "codebar.h"
+#include "pawn_sensor.h"
+#include "radar.h"
#define FSM_NAME AI
#include "fsm.h"
#ifdef HOST
# include <string.h>
#endif
+#include "fsm_queue.h"
#include "clamp.h"
+#include "logistic.h"
+#include "path.h"
+#include "move.h"
+
+#include "bot.h"
#include "io.h"
+#ifndef HOST
+# include <avr/wdt.h>
+#endif
+
/** Our color. */
enum team_color_e team_color;
+/** Obstacles positions, updated using radar module. */
+vect_t main_obstacles_pos[2];
+
+/** Number of obstacles in main_obstacles_pos. */
+uint8_t main_obstacles_nb;
+
/** Asserv stats counters. */
static uint8_t main_stats_asserv_, main_stats_asserv_cpt_;
/** Contact stats counters. */
static uint8_t main_stats_contact_, main_stats_contact_cpt_;
+/** Codebar stats counters. */
+static uint8_t main_stats_codebar_, main_stats_codebar_cpt_;
+
+/** US sensors stats counters. */
+static uint8_t main_stats_usdist_, main_stats_usdist_cpt_;
+
/** Main initialisation. */
static void
main_init (void)
{
+#ifndef HOST
+ /* Disable watchdog (enabled in bootloader). */
+ MCUSR &= ~(1 << WDRF);
+ wdt_disable ();
+#endif
/* Serial port */
uart0_init ();
/* Enable interrupts */
@@ -76,6 +108,12 @@ main_init (void)
/* IO modules. */
pwm_init ();
contact_init ();
+ codebar_init ();
+ usdist_init ();
+ /* AI modules. */
+ clamp_init ();
+ logistic_init ();
+ path_init ();
/* Initialization done. */
proto_send0 ('z');
}
@@ -93,18 +131,52 @@ main_event_to_fsm (void)
#define FSM_HANDLE_TIMEOUT_E(fsm) \
do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0)
/* Update FSM timeouts. */
- //FSM_HANDLE_TIMEOUT_E (AI);
+ FSM_HANDLE_TIMEOUT_E (AI);
/* Motor status. */
- asserv_status_e mimot_motor0_status, mimot_motor1_status;
+ asserv_status_e robot_move_status, mimot_motor0_status,
+ mimot_motor1_status;
+ robot_move_status = asserv_move_cmd_status ();
mimot_motor0_status = mimot_motor0_cmd_status ();
mimot_motor1_status = mimot_motor1_cmd_status ();
+ if (robot_move_status == success)
+ FSM_HANDLE_E (AI, robot_move_success);
+ else if (robot_move_status == failure)
+ FSM_HANDLE_E (AI, robot_move_failure);
if (mimot_motor0_status == success
&& mimot_motor1_status == success)
FSM_HANDLE_E (AI, clamp_elevation_rotation_success);
+ if (mimot_motor0_status == failure
+ || mimot_motor1_status == failure)
+ FSM_HANDLE_E (AI, clamp_elevation_or_rotation_failure);
+ if (mimot_motor0_status == success)
+ FSM_HANDLE_E (AI, clamp_elevation_success);
else if (mimot_motor0_status == failure)
FSM_HANDLE_E (AI, clamp_elevation_failure);
+ if (mimot_motor1_status == success)
+ FSM_HANDLE_E (AI, clamp_rotation_success);
else if (mimot_motor1_status == failure)
FSM_HANDLE_E (AI, clamp_rotation_failure);
+ /* Clamp specific events. */
+ if (clamp_handle_event ())
+ return;
+ /* Jack. */
+ if (!contact_get_jack ())
+ FSM_HANDLE_E (AI, jack_inserted);
+ else
+ FSM_HANDLE_E (AI, jack_removed);
+ /* Events from the event queue. */
+ if (fsm_queue_poll ())
+ {
+ /* We must post the event at the end of this block because if it is
+ * handled, the function will return and every instruction after will
+ * never be executed. */
+ uint8_t save_event = fsm_queue_pop_event ();
+ /* Post the event */
+ FSM_HANDLE_VAR_E (AI, save_event);
+ }
+ /* Check obstables. */
+ if (move_check_obstacles ())
+ return;
}
/** Main (and infinite) loop. */
@@ -120,6 +192,12 @@ main_loop (void)
/* Is match over? */
if (chrono_is_match_over ())
{
+ /* Power off doors. */
+ pwm_set (BOT_PWM_DOOR_FRONT_BOTTOM, 0);
+ pwm_set (BOT_PWM_DOOR_FRONT_TOP, 0);
+ pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, 0);
+ pwm_set (BOT_PWM_DOOR_BACK_TOP, 0);
+ pwm_set (BOT_PWM_CLAMP, 0);
/* End it and block here indefinitely. */
chrono_end_match (42);
return;
@@ -130,6 +208,18 @@ main_loop (void)
/* Update IO modules. */
pwm_update ();
contact_update ();
+ pawn_sensor_update ();
+ if (usdist_update ())
+ {
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos);
+ move_obstacles_update ();
+ simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0);
+ }
+ /* Update AI modules. */
+ logistic_update ();
+ path_decay ();
/* Only manage events if slaves are synchronised. */
if (twi_master_sync ())
main_event_to_fsm ();
@@ -146,9 +236,21 @@ main_loop (void)
}
if (main_stats_contact_ && !--main_stats_contact_cpt_)
{
- proto_send1d ('P', contact_all ());
+ proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24);
main_stats_contact_cpt_ = main_stats_contact_;
}
+ if (main_stats_codebar_ && !--main_stats_codebar_cpt_)
+ {
+ proto_send2b ('B', codebar_get (DIRECTION_FORWARD),
+ codebar_get (DIRECTION_BACKWARD));
+ main_stats_codebar_cpt_ = main_stats_codebar_;
+ }
+ if (main_stats_usdist_ && !--main_stats_usdist_cpt_)
+ {
+ proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2],
+ usdist_mm[3]);
+ main_stats_usdist_cpt_ = main_stats_usdist_;
+ }
}
}
@@ -163,6 +265,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Reset */
utils_reset ();
break;
+ case c ('j', 0):
+ /* Simulate jack insertion. */
+ fsm_queue_post_event (FSM_EVENT (AI, jack_inserted));
+ break;
case c ('w', 3):
/* Set PWM.
* - 1b: index.
@@ -179,11 +285,64 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v16 (args[3], args[4]),
v8_to_v16 (args[5], args[6]));
break;
+ case c ('m', 5):
+ /* Go to position.
+ * - 2w: x, y.
+ * - 1b: backward. */
+ {
+ vect_t position = { v8_to_v16 (args[0], args[1]),
+ v8_to_v16 (args[2], args[3]) };
+ move_start_noangle (position, args[4], 0);
+ }
+ break;
case c ('c', 1):
/* Move clamp.
* - 1b: position. */
clamp_move (args[0]);
break;
+ case c ('c', 2):
+ /* Move element using clamp.
+ * - 1b: source.
+ * - 1b: destination. */
+ clamp_move_element (args[0], args[1]);
+ break;
+ case c ('n', 2):
+ /* Simulate the presence of a new element.
+ * - 1b: position.
+ * - 1b: type. */
+ clamp_new_element (args[0], args[1]);
+ break;
+ case c ('d', 0):
+ /* Open all doors. */
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_BOTTOM));
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_TOP));
+ pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_BOTTOM));
+ pwm_set_timed (BOT_PWM_DOOR_BACK_TOP,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_TOP));
+ break;
+ case c ('d', 1):
+ /* Drop elements.
+ * - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */
+ if (args[0] == 0x00)
+ clamp_drop_clear ();
+ else
+ clamp_drop (args[0]);
+ break;
+ case c ('d', 2):
+ /* Open or close door or clamp.
+ * - 1b: pos, or 0xff for clamp.
+ * - 1b: non zero to open. */
+ clamp_door (args[0], args[1]);
+ break;
+ case c ('w', 0):
+ /* Disable all motor control. */
+ mimot_motor0_free ();
+ mimot_motor1_free ();
+ asserv_free_motor ();
+ break;
/* Stats commands.
* - b: interval between stats. */
case c ('A', 1):
@@ -194,6 +353,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Contact stats. */
main_stats_contact_ = main_stats_contact_cpt_ = args[0];
break;
+ case c ('B', 1):
+ /* Codebar stats. */
+ main_stats_codebar_ = main_stats_codebar_cpt_ = args[0];
+ break;
+ case c ('U', 1):
+ /* US sensors stats. */
+ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0];
+ break;
default:
/* Unknown commands */
proto_send0 ('?');
diff --git a/digital/io-hub/src/robospierre/main.h b/digital/io-hub/src/robospierre/main.h
new file mode 100644
index 00000000..529aa0b2
--- /dev/null
+++ b/digital/io-hub/src/robospierre/main.h
@@ -0,0 +1,31 @@
+#ifndef main_h
+#define main_h
+/* main.h */
+/* 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.
+ *
+ * }}} */
+
+extern vect_t main_obstacles_pos[2];
+extern uint8_t main_obstacles_nb;
+
+#endif /* main_h */
diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c
new file mode 100644
index 00000000..a44b5715
--- /dev/null
+++ b/digital/io-hub/src/robospierre/move.c
@@ -0,0 +1,541 @@
+/* move.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 "move.h"
+#include "playground_2011.h"
+
+#include "main.h"
+#include "asserv.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
+
+#include "radar.h"
+#include "path.h"
+
+#include "modules/utils/utils.h"
+
+#include <math.h>
+
+/** Move context. */
+struct move_t
+{
+ /** Final position. */
+ position_t final;
+ /** Use angle consign for final point. */
+ uint8_t with_angle;
+ /** Next step. */
+ vect_t step;
+ /** Next step angle. */
+ uint16_t step_angle;
+ /** Next step with_angle. */
+ uint8_t step_with_angle;
+ /** Next step backward. */
+ uint8_t step_backward;
+ /** Non zero means this is a tricky move, slow down, and minimize
+ * turns. */
+ uint8_t slow;
+ /** Backward direction allowed flag. */
+ uint8_t backward_movement_allowed;
+ /** Try again counter. */
+ uint8_t try_again_counter;
+ /** Dirty fix to know this is the final move. */
+ uint8_t final_move;
+ /** Distance to remove from path. */
+ int16_t shorten;
+};
+
+/* Global context. */
+struct move_t move_data;
+
+void
+move_start (position_t position, uint8_t backward)
+{
+ /* Set parameters. */
+ move_data.final = position;
+ move_data.with_angle = 1;
+ move_data.backward_movement_allowed = backward;
+ move_data.final_move = 0;
+ move_data.shorten = 0;
+ /* Reset try counter. */
+ move_data.try_again_counter = 3;
+ /* Start the FSM. */
+ fsm_queue_post_event (FSM_EVENT (AI, move_start));
+}
+
+void
+move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
+{
+ /* Set parameters. */
+ move_data.final.v = position;
+ move_data.with_angle = 0;
+ move_data.backward_movement_allowed = backward;
+ move_data.final_move = 0;
+ move_data.shorten = shorten;
+ /* Reset try counter. */
+ move_data.try_again_counter = 3;
+ /* Start the FSM. */
+ fsm_queue_post_event (FSM_EVENT (AI, move_start));
+}
+
+void
+move_stop (void)
+{
+ /* Stop the FSM. */
+ fsm_queue_post_event (FSM_EVENT (AI, move_stop));
+}
+
+void
+move_obstacles_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < main_obstacles_nb; i++)
+ path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0,
+ MOVE_OBSTACLE_VALIDITY);
+}
+
+uint8_t
+move_check_obstacles (void)
+{
+ if (FSM_CAN_HANDLE (AI, obstacle_in_front))
+ {
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos,
+ main_obstacles_nb))
+ if (FSM_HANDLE (AI, obstacle_in_front))
+ return 1;
+ }
+ return 0;
+}
+
+FSM_STATES (
+ /* Waiting for the start order. */
+ MOVE_IDLE,
+ /* Rotating towards next point. */
+ MOVE_ROTATING,
+ /* Moving to a position (intermediate or final). */
+ MOVE_MOVING,
+ /* Moving backward to go away from what is blocking the bot. */
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY,
+ /* Waiting for obstacle to disappear. */
+ MOVE_WAIT_FOR_CLEAR_PATH)
+
+FSM_EVENTS (
+ /* Report from asserv after a successful move command. */
+ robot_move_success,
+ /* Report from asserv after a failed move command. */
+ robot_move_failure,
+ /* Initialize the FSM and start the movement directly. */
+ move_start,
+ /* Stop movement. */
+ move_stop,
+ /* Movement success. */
+ move_success,
+ /* Movement failure. */
+ move_failure,
+ /* The bot has seen something (front is the same when going backward). */
+ obstacle_in_front)
+
+FSM_START_WITH (MOVE_IDLE)
+
+/** Go to current step, low level function. */
+static void
+move_go (void)
+{
+ vect_t dst = move_data.step;
+ /* Modify final point if requested. */
+ if (move_data.final_move && move_data.shorten)
+ {
+ /* Compute a vector from destination to robot with lenght
+ * 'shorten'. */
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ vect_t v = robot_position.v;
+ vect_sub (&v, &move_data.step);
+ int16_t d = vect_norm (&v);
+ if (d > move_data.shorten)
+ {
+ vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten);
+ vect_translate (&dst, &v);
+ }
+ }
+ if (move_data.step_with_angle)
+ asserv_goto_xya (dst.x, dst.y, move_data.step_angle,
+ move_data.step_backward);
+ else
+ asserv_goto (dst.x, dst.y, move_data.step_backward);
+}
+
+/** Go or rotate toward position, returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
+ uint8_t backward)
+{
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ uint16_t robot_angle = robot_position.a;
+ if (backward & ASSERV_BACKWARD)
+ robot_angle += 0x8000;
+ /* Check for green zone. */
+ if (robot_position.v.x < PG_GREEN_WIDTH_MM
+ || robot_position.v.x > PG_WIDTH - PG_GREEN_WIDTH_MM)
+ backward |= ASSERV_REVERT_OK;
+ /* Remember step. */
+ move_data.step = dst;
+ move_data.step_angle = angle;
+ move_data.step_with_angle = with_angle;
+ move_data.step_backward = backward;
+ /* Compute angle to destination. */
+ vect_t v = dst; vect_sub (&v, &robot_position.v);
+ uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
+ if (backward & ASSERV_BACKWARD)
+ dst_angle += 0x8000;
+ int16_t diff_angle = dst_angle - robot_angle;
+ if ((backward & ASSERV_REVERT_OK)
+ && (diff_angle > 0x4000 || diff_angle < -0x4000))
+ dst_angle += 0x8000;
+ int16_t diff = dst_angle - robot_angle;
+ /* Move or rotate. */
+ if (UTILS_ABS (diff) < 0x1000)
+ {
+ move_go ();
+ return 1;
+ }
+ else
+ {
+ asserv_goto_angle (dst_angle);
+ return 2;
+ }
+}
+
+/** Go to next position computed by path module, to be called by
+ * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_to_next (vect_t dst)
+{
+ uint8_t r;
+ /* If it is not the last position. */
+ if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y)
+ {
+ /* Not final position. */
+ move_data.final_move = 0;
+ /* Goto without angle. */
+ r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed
+ | (move_data.slow ? ASSERV_REVERT_OK : 0));
+ }
+ else
+ {
+ /* Final position. */
+ move_data.final_move = 1;
+ /* Goto with angle if requested. */
+ r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle,
+ move_data.backward_movement_allowed);
+ }
+ /* Next time, do not use slow. */
+ move_data.slow = 0;
+ return r;
+}
+
+/** Update and go to first position, return non zero if a path is found, 1 for
+ * linear move, 2 for angular move. */
+static uint8_t
+move_path_init (void)
+{
+ uint8_t found;
+ vect_t dst;
+ /* Get the current position */
+ position_t current_pos;
+ asserv_get_position (&current_pos);
+ /* Give the current position of the bot to the path module */
+ path_endpoints (current_pos.v, move_data.final.v);
+ /* Update the path module */
+ move_data.slow = 0;
+ path_update ();
+ found = path_get_next (&dst);
+ /* If not found, try to escape. */
+ if (!found)
+ {
+ move_data.slow = 1;
+ path_escape (8);
+ path_update ();
+ found = path_get_next (&dst);
+ }
+ /* If found, go. */
+ if (found)
+ {
+ return move_go_to_next (dst);
+ }
+ else
+ {
+ /* Error, not final move. */
+ move_data.final_move = 0;
+ return 0;
+ }
+}
+
+/** Go to next position in path. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_path_next (void)
+{
+ vect_t dst;
+ path_get_next (&dst);
+ return move_go_to_next (dst);
+}
+
+FSM_TRANS (MOVE_IDLE, move_start,
+ path_found_rotate, MOVE_ROTATING,
+ path_found, MOVE_MOVING,
+ no_path_found, MOVE_IDLE)
+{
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_IDLE, move_start, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_IDLE, move_start, no_path_found);
+ }
+}
+
+FSM_TRANS (MOVE_ROTATING,
+ robot_move_success,
+ MOVE_MOVING)
+{
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, robot_move_success);
+}
+
+FSM_TRANS (MOVE_ROTATING,
+ robot_move_failure,
+ MOVE_MOVING)
+{
+ move_go ();
+ return FSM_NEXT (MOVE_ROTATING, robot_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250,
+ MOVE_MOVING)
+{
+ move_go ();
+ return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
+}
+
+FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_ROTATING, move_stop);
+}
+
+FSM_TRANS (MOVE_MOVING, robot_move_success,
+ done, MOVE_IDLE,
+ path_found_rotate, MOVE_ROTATING,
+ path_found, MOVE_MOVING,
+ no_path_found, MOVE_IDLE)
+{
+ if (move_data.final_move)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_success));
+ return FSM_NEXT (MOVE_MOVING, robot_move_success, done);
+ }
+ else
+ {
+ uint8_t next = move_path_next ();
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found);
+ }
+ //return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found);
+}
+
+static void
+move_moving_backward_to_turn_freely (void)
+{
+ move_data.final_move = 0;
+ /* Assume there is an obstacle in front of the robot. */
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ vect_t obstacle_pos;
+ int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD
+ ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
+ : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
+ vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
+ vect_translate (&obstacle_pos, &robot_pos.v);
+ path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
+ MOVE_OBSTACLE_VALIDITY);
+ /* Move backward to turn freely. */
+ asserv_move_linearly (asserv_get_last_moving_direction ()
+ == DIRECTION_FORWARD ? -300 : 300);
+}
+
+FSM_TRANS (MOVE_MOVING,
+ robot_move_failure,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_moving_backward_to_turn_freely ();
+ return FSM_NEXT (MOVE_MOVING, robot_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500,
+ MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
+{
+ move_moving_backward_to_turn_freely ();
+ return FSM_NEXT_TIMEOUT (MOVE_MOVING);
+}
+
+FSM_TRANS (MOVE_MOVING, obstacle_in_front,
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ tryout, MOVE_IDLE)
+{
+ move_data.final_move = 0;
+ asserv_stop_motor ();
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+}
+
+FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING, move_stop);
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
+ tryout, MOVE_IDLE,
+ path_found_rotate, MOVE_ROTATING,
+ path_found, MOVE_MOVING,
+ no_path_found, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found);
+ }
+ else
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, no_path_found);
+ }
+ }
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
+ tryout, MOVE_IDLE,
+ path_found_rotate, MOVE_ROTATING,
+ path_found, MOVE_MOVING,
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ no_path_found_tryout, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, tryout);
+ }
+ else
+ {
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found_rotate);
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found);
+ }
+ else
+ {
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryagain);
+ }
+ }
+}
+
+FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE)
+{
+ asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop);
+}
+
+FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
+ path_found_rotate, MOVE_ROTATING,
+ path_found, MOVE_MOVING,
+ no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ no_path_found_tryout, MOVE_IDLE)
+{
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate);
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found);
+ }
+ else
+ {
+ /* Error, no new position, should we try again? */
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout);
+ }
+ else
+ return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain);
+ }
+}
+
+FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE)
+{
+ return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop);
+}
+
diff --git a/digital/io-hub/src/robospierre/move.h b/digital/io-hub/src/robospierre/move.h
new file mode 100644
index 00000000..72af5b84
--- /dev/null
+++ b/digital/io-hub/src/robospierre/move.h
@@ -0,0 +1,63 @@
+#ifndef move_h
+#define move_h
+/* move.h */
+/* 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 "defs.h"
+#include "bot.h"
+
+/** Real radius of an obstacle. */
+#define MOVE_REAL_OBSTACLE_RADIUS 150
+
+/** Obstacle radius for the path module.
+ * It corresponds to the real radius of the obstacle plus the distance you
+ * want to add to avoid it. */
+#define MOVE_OBSTACLE_RADIUS (MOVE_REAL_OBSTACLE_RADIUS \
+ + RADAR_CLEARANCE_MM \
+ + BOT_SIZE_SIDE)
+
+/** Obstacle validity time (in term of number of cycles). */
+#define MOVE_OBSTACLE_VALIDITY (3 * 225)
+
+/** Go to a position, see asserv.h for backward argument. */
+void
+move_start (position_t position, uint8_t backward);
+
+/** Go to a position, with no angle consign. */
+void
+move_start_noangle (vect_t position, uint8_t backward, int16_t shorten);
+
+/** Stop movement. */
+void
+move_stop (void);
+
+/** To be called when obstacles positions are computed. */
+void
+move_obstacles_update (void);
+
+/** Check for blocking obstacles, return non zero if an event is handled. */
+uint8_t
+move_check_obstacles (void);
+
+#endif /* move_h */
diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c
new file mode 100644
index 00000000..ecdddbf7
--- /dev/null
+++ b/digital/io-hub/src/robospierre/path.c
@@ -0,0 +1,621 @@
+/* path.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 "defs.h"
+#include "path.h"
+#include "bot.h"
+#include "playground_2011.h"
+#include "element.h"
+
+#include "modules/path/astar/astar.h"
+#include "modules/utils/utils.h"
+#include "modules/math/geometry/distance.h"
+
+#define PATH_DEBUG 0
+
+#if PATH_DEBUG
+#include "debug.host.h"
+#endif
+
+/**
+ * This year, due to the large number of obstacles, a grid like structure is
+ * used for path finding on the playground. The A* algorithm is used to find
+ * path along nodes.
+ *
+ * The grid is composed of 11 columns of 4 node each. They are numbered by
+ * column. Even columns are aligned with center of squares, while odd columns
+ * are at squares intersections. Therefore, odd columns have a offset of
+ * 352/2 mm, and that is the reason why code should handle odd and even
+ * columns differently.
+ *
+ * There is also extra grid nodes in front of the green zone.
+ *
+ * All those tricks are used to reduce the number of nodes.
+ */
+
+/** Number of possible obstacles. */
+#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB
+
+/** Number of nodes in a column. */
+#define PATH_COLUMN_NODES_NB 4
+
+/** Number of columns. */
+#define PATH_COLUMNS_NB 11
+
+/** Number of nodes in the grid. */
+#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB)
+
+/** Number of nodes in front of each green zone. */
+#define PATH_GREEN_NODES_NB 4
+
+/** Number of fixed nodes. */
+#define PATH_FIXED_NODES_NB (PATH_GRID_NODES_NB + 2 * PATH_GREEN_NODES_NB)
+
+/** Number of nodes in search graph, last two nodes are destination and source
+ * nodes. */
+#define PATH_NODES_NB (PATH_FIXED_NODES_NB + 2)
+
+/** Index of destination node. */
+#define PATH_DST_NODE_INDEX PATH_FIXED_NODES_NB
+
+/** Index of source node. */
+#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1)
+
+/** Information on a node. */
+struct path_node_t
+{
+ /** Whether this node can be used. */
+ uint8_t usable;
+};
+
+/** Context. */
+struct path_t
+{
+ /** List of obstacles. */
+ struct path_obstacle_t obstacles[PATH_OBSTACLES_NB];
+ /** Escape factor, 0 if none. */
+ uint8_t escape_factor;
+ /** List of nodes used for A*. */
+ struct astar_node_t astar_nodes[PATH_NODES_NB];
+ /** Cache of whether a node is blocked. */
+ uint8_t valid[PATH_FIXED_NODES_NB];
+ /** Position of end points. */
+ vect_t endpoints[2];
+ /** Whether the last update was a success. */
+ uint8_t found;
+ /** Which node to look at for next step. */
+ uint8_t get;
+};
+static struct path_t path;
+
+/** Static information on nodes. */
+static const struct path_node_t path_nodes[PATH_FIXED_NODES_NB] = {
+ /* {{{ */
+ { 1 }, /* 0 column 0. */
+ { 1 }, /* 1 */
+ { 1 }, /* 2 */
+ { 1 }, /* 3 */
+ { 1 }, /* 4 column 1. */
+ { 1 }, /* 5 */
+ { 1 }, /* 6 */
+ { 1 }, /* 7 */
+ { 1 }, /* 8 column 2. */
+ { 1 }, /* 9 */
+ { 1 }, /* 10 */
+ { 1 }, /* 11 */
+ { 1 }, /* 12 column 3. */
+ { 1 }, /* 13 */
+ { 1 }, /* 14 */
+ { 1 }, /* 15 */
+ { 1 }, /* 16 column 4. */
+ { 1 }, /* 17 */
+ { 1 }, /* 18 */
+ { 1 }, /* 19 */
+ { 1 }, /* 20 column 5. */
+ { 1 }, /* 21 */
+ { 1 }, /* 22 */
+ { 1 }, /* 23 */
+ { 1 }, /* 24 column 6. */
+ { 1 }, /* 25 */
+ { 1 }, /* 26 */
+ { 1 }, /* 27 */
+ { 1 }, /* 28 column 7. */
+ { 1 }, /* 29 */
+ { 1 }, /* 30 */
+ { 1 }, /* 31 */
+ { 1 }, /* 32 column 8. */
+ { 1 }, /* 33 */
+ { 1 }, /* 34 */
+ { 1 }, /* 35 */
+ { 1 }, /* 36 column 9. */
+ { 1 }, /* 37 */
+ { 1 }, /* 38 */
+ { 1 }, /* 39 */
+ { 1 }, /* 40 column 10. */
+ { 1 }, /* 41 */
+ { 1 }, /* 42 */
+ { 1 }, /* 43 */
+ { 1 }, /* 44 left green. */
+ { 1 }, /* 45 */
+ { 1 }, /* 46 */
+ { 1 }, /* 47 */
+ { 1 }, /* 48 right green. */
+ { 1 }, /* 49 */
+ { 1 }, /* 50 */
+ { 1 }, /* 51 */
+ /* }}} */
+};
+
+/** Compute position of a node. */
+static void
+path_pos (uint8_t node, vect_t *pos)
+{
+ assert (node < PATH_NODES_NB);
+ if (node < PATH_GRID_NODES_NB)
+ {
+ uint8_t col = node / PATH_COLUMN_NODES_NB;
+ uint8_t line = node - col * PATH_COLUMN_NODES_NB;
+ pos->x = 400 + 50 + 350 / 2 + col * 350 / 2;
+ pos->y = 2100 - 350 - 350 / 2
+ + (col % 2 ? 350 / 2 : 0)
+ - line * 350;
+ }
+ else if (node < PATH_GRID_NODES_NB + 2 * PATH_GREEN_NODES_NB)
+ {
+ node -= PATH_GRID_NODES_NB;
+ uint8_t col = node / PATH_GREEN_NODES_NB;
+ uint8_t line = node - col * PATH_GREEN_NODES_NB;
+ pos->x = col == 0 ? BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
+ : PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM;
+ pos->y = (5 - line) * 280 + 10;
+ }
+ else
+ {
+ *pos = path.endpoints[node - PATH_FIXED_NODES_NB];
+ }
+}
+
+static uint8_t
+path_element_blocking (uint8_t node, uint8_t escape)
+{
+ vect_t pos;
+ path_pos (node, &pos);
+ int16_t square_x = (pos.x - 450 - 1) / 350;
+ int16_t square_y = (2100 - pos.y - 1) / 350;
+ uint8_t element_id = ELEMENT_UNLOAD_START + square_x + 6 * square_y;
+ if (element_blocking (element_id, escape))
+ return 1;
+ uint8_t intersection = ((pos.x - 450) / 350) != square_x;
+ if (intersection)
+ {
+ if (element_blocking (element_id + 1, escape))
+ return 1;
+ if (element_blocking (element_id + 6, escape))
+ return 1;
+ if (element_blocking (element_id + 6 + 1, escape))
+ return 1;
+ }
+ return 0;
+}
+
+/** Return 1 if the direct path between a and b nodes is blocked, also compute
+ * distance. */
+static uint8_t
+path_blocking (uint8_t a, uint8_t b, int16_t *dp)
+{
+ uint8_t i;
+ vect_t va;
+ vect_t vb;
+ uint8_t escape_factor = 0;
+ uint8_t factor = 1;
+ uint8_t blocking = 0;
+ if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX)
+ escape_factor = path.escape_factor;
+ path_pos (a, &va);
+ path_pos (b, &vb);
+ /* Test for green zone. */
+ uint8_t a_green, b_green;
+ a_green = va.x < PG_GREEN_WIDTH_MM || va.x > PG_WIDTH - PG_GREEN_WIDTH_MM;
+ b_green = vb.x < PG_GREEN_WIDTH_MM || vb.x > PG_WIDTH - PG_GREEN_WIDTH_MM;
+ if ((va.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
+ && vb.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)
+ || (va.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
+ && vb.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)
+ || (va.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
+ && vb.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)
+ || (va.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM
+ && vb.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM))
+ return 1;
+ if (a_green && b_green)
+ return 1;
+ if (a_green || b_green)
+ factor = 4;
+ /* Test for protected zone. */
+ if (va.y <= 350 && va.x > PG_WIDTH / 2 - 350 && va.y < PG_WIDTH / 2 + 350
+ && (vb.x < PG_WIDTH / 2 - 350 || vb.x > PG_WIDTH / 2 + 350))
+ return 1;
+ if (vb.y <= 350 && vb.x > PG_WIDTH / 2 - 350 && vb.y < PG_WIDTH / 2 + 350
+ && (va.x < PG_WIDTH / 2 - 350 || va.x > PG_WIDTH / 2 + 350))
+ return 1;
+ /* Test for a blocking obstacle. */
+ for (i = 0; i < PATH_OBSTACLES_NB && !blocking; i++)
+ {
+ if (path.obstacles[i].valid)
+ {
+ uint16_t d = distance_segment_point (&va, &vb,
+ &path.obstacles[i].c);
+ if (d < path.obstacles[i].r)
+ blocking = 1;
+ }
+ }
+ /* Compute distance. */
+ int16_t d = distance_point_point (&va, &vb);
+ if (d == 0)
+ {
+ *dp = 0;
+ return 0;
+ }
+ /* Test for a blocking element. */
+ if (element_blocking_path (va, vb, d, path.escape_factor))
+ blocking = 1;
+ /* Handle escaping. */
+ if (blocking)
+ {
+ if (escape_factor)
+ {
+ *dp = d * escape_factor;
+ return 0;
+ }
+ else
+ return 1;
+ }
+ /* No blocking. */
+ *dp = d * factor;
+ return 0;
+}
+
+/** Update the cache of blocked nodes. */
+static void
+path_blocked_update (void)
+{
+ uint8_t i, j;
+ for (i = 0; i < PATH_FIXED_NODES_NB; i++)
+ {
+ uint8_t valid = 1;
+ /* First, gather information from tables. */
+ if (!path_nodes[i].usable
+ || path_element_blocking (i, path.escape_factor))
+ valid = 0;
+ else
+ {
+ vect_t pos;
+ path_pos (i, &pos);
+ /* Then, test for obstacles. */
+ for (j = 0; j < PATH_OBSTACLES_NB; j++)
+ {
+ if (path.obstacles[j].valid)
+ {
+ vect_t v = pos; vect_sub (&v, &path.obstacles[j].c);
+ uint32_t dsq = vect_dot_product (&v, &v);
+ uint32_t r = path.obstacles[j].r;
+ if (dsq <= r * r)
+ {
+ valid = 0;
+ break;
+ }
+ }
+ }
+ }
+ /* Update cache. */
+ path.valid[i] = valid;
+ }
+}
+
+void
+path_init (void)
+{
+}
+
+void
+path_endpoints (vect_t s, vect_t d)
+{
+ path.endpoints[0] = d;
+ path.endpoints[1] = s;
+}
+
+void
+path_escape (uint8_t factor)
+{
+ path.escape_factor = factor;
+}
+
+void
+path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor,
+ uint16_t valid)
+{
+ assert (i < AC_PATH_OBSTACLES_NB);
+ assert (factor == 0);
+ path.obstacles[i].c = c;
+ path.obstacles[i].r = r;
+ path.obstacles[i].valid = valid;
+}
+
+void
+path_decay (void)
+{
+ uint8_t i;
+ for (i = 0; i < PATH_OBSTACLES_NB; i++)
+ {
+ if (path.obstacles[i].valid
+ && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS)
+ path.obstacles[i].valid--;
+ }
+}
+
+void
+path_update (void)
+{
+ path_blocked_update ();
+ path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX,
+ PATH_SRC_NODE_INDEX);
+ path.get = PATH_SRC_NODE_INDEX;
+#if AC_PATH_REPORT
+ if (path.found)
+ {
+ uint8_t n, len = 0;
+ vect_t points[PATH_NODES_NB];
+ for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev)
+ path_pos (n, &points[len++]);
+ path_pos (n, &points[len++]);
+ AC_PATH_REPORT_CALLBACK (points, len, path.obstacles,
+ PATH_OBSTACLES_NB);
+ }
+#endif
+}
+
+uint8_t
+path_get_next (vect_t *p)
+{
+ if (path.found)
+ {
+ assert (path.get != PATH_DST_NODE_INDEX);
+ uint8_t prev = path.get;
+ vect_t pp;
+ path_pos (prev, &pp);
+ uint8_t next = path.astar_nodes[path.get].prev;
+ path.get = next;
+ path_pos (next, p);
+ while (next != 0xff)
+ {
+ /* Try to remove useless points. */
+ uint8_t next = path.astar_nodes[path.get].prev;
+ if (next == 0xff || next == PATH_DST_NODE_INDEX)
+ break;
+ vect_t np;
+ path_pos (next, &np);
+ vect_t vnp = np; vect_sub (&vnp, &pp);
+ vect_t vp = *p; vect_sub (&vp, &pp);
+ if (vect_normal_dot_product (&vp, &vnp) == 0)
+ {
+ path.get = next;
+ *p = np;
+ }
+ else
+ break;
+ }
+ return 1;
+ }
+ else
+ return 0;
+}
+
+/** Neighbors callback for nodes in grid. */
+static uint8_t
+path_astar_neighbor_callback_grid (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ /* Add neighbors in all 8 directions. */
+ static const struct {
+ /** Column offset of this neighbor. */
+ int8_t column_offset;
+ /** Line offset of this neighbor, for even columns. */
+ int8_t even_line_offset;
+ /** Line offset for odd columns. */
+ int8_t odd_line_offset;
+ /** Distance to this neighbor. */
+ uint16_t weight;
+ } star_n[] = {
+ { 0, -1, -1, 350 }, /* N */
+ { -1, 0, -1, 248 }, /* NW */
+ { -2, 0, 0, 350 }, /* W */
+ { -1, 1, 0, 248 }, /* SW */
+ { 0, 1, 1, 350 }, /* S */
+ { 1, 1, 0, 248 }, /* SE */
+ { 2, 0, 0, 350 }, /* E */
+ { 1, 0, -1, 248 }, /* NE */
+ };
+ uint8_t col = node / PATH_COLUMN_NODES_NB;
+ uint8_t line = node - col * PATH_COLUMN_NODES_NB;
+ uint8_t odd = col % 2;
+ for (i = 0; i < UTILS_COUNT (star_n); i++)
+ {
+ int8_t new_col = col + star_n[i].column_offset;
+ int8_t new_line = line + (odd ? star_n[i].odd_line_offset
+ : star_n[i].even_line_offset);
+ int8_t new_node = new_col * PATH_COLUMN_NODES_NB + new_line;
+ if (new_col >= 0 && new_col < PATH_COLUMNS_NB
+ && new_line >= 0 && new_line < PATH_COLUMN_NODES_NB)
+ {
+ uint8_t valid = path.valid[new_node];
+ if (valid)
+ {
+ neighbors[neighbors_nb].node = new_node;
+ neighbors[neighbors_nb].weight = star_n[i].weight + 1;
+ neighbors_nb++;
+ }
+ }
+ }
+ /* Check path to green nodes. */
+ int16_t d;
+ if (col <= 1 || col >= PATH_COLUMNS_NB - 1)
+ {
+ uint8_t green = PATH_GRID_NODES_NB
+ + (col <= 1 ? 0 : PATH_GREEN_NODES_NB);
+ for (i = green; i < green + PATH_GREEN_NODES_NB; i++)
+ {
+ if (!path_blocking (node, i, &d))
+ {
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+ }
+ }
+ /* Check if direct path OK. */
+ if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+/** Neighbors callback for green nodes. */
+static uint8_t
+path_astar_neighbor_callback_green (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ uint8_t col = (node - PATH_GRID_NODES_NB) / PATH_GREEN_NODES_NB;
+ int16_t d;
+ /* Check path to grid nodes. */
+ uint8_t grid = col ? PATH_GRID_NODES_NB - 2 * PATH_COLUMN_NODES_NB : 0;
+ for (i = grid; i < grid + 2 * PATH_COLUMN_NODES_NB; i++)
+ {
+ if (!path_blocking (node, i, &d))
+ {
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+ }
+ /* Check path to other green nodes. */
+ uint8_t green = PATH_GRID_NODES_NB + (col ? PATH_GREEN_NODES_NB : 0);
+ for (i = green; i < green + PATH_GREEN_NODES_NB; i++)
+ {
+ if (i != node && !path_blocking (node, i, &d))
+ {
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+ }
+ /* Check if direct path OK. */
+ if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+/** Neighbors callback for endpoints. */
+static uint8_t
+path_astar_neighbor_callback_endpoints (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ assert (node == PATH_DST_NODE_INDEX);
+ /* Select neighbors in the fixed nodes. */
+ for (i = 0; i < PATH_FIXED_NODES_NB; i++)
+ {
+ /* Discard blocking nodes. */
+ if (!path.valid[i])
+ continue;
+ /* Check if there is an obstacle along the path. */
+ int16_t d;
+ if (path_blocking (PATH_DST_NODE_INDEX, i, &d))
+ continue;
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = i;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+ /* Check if direct path OK. */
+ int16_t d;
+ if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d))
+ {
+ /* Add this neighbor. */
+ neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX;
+ neighbors[neighbors_nb].weight = d + 1;
+ neighbors_nb++;
+ }
+#if PATH_DEBUG
+ for (i = 0; i < neighbors_nb; i++)
+ DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight);
+#endif
+ return neighbors_nb;
+}
+
+uint8_t
+path_astar_neighbor_callback (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+#if PATH_DEBUG
+ DPRINTF ("neighbor %d\n", node);
+#endif
+ if (node < PATH_GRID_NODES_NB)
+ return path_astar_neighbor_callback_grid (node, neighbors);
+ else if (node < PATH_GRID_NODES_NB + 2 * PATH_GREEN_NODES_NB)
+ return path_astar_neighbor_callback_green (node, neighbors);
+ else
+ return path_astar_neighbor_callback_endpoints (node, neighbors);
+}
+
+uint16_t
+path_astar_heuristic_callback (uint8_t node)
+{
+ /* TODO: a better and faster heuristic can be found, considering that
+ * movement is only allowed on the grid. */
+ vect_t pos;
+ path_pos (node, &pos);
+ return distance_point_point (&pos, &path.endpoints[0]);
+}
diff --git a/digital/io-hub/src/robospierre/path.h b/digital/io-hub/src/robospierre/path.h
new file mode 100644
index 00000000..22a8ead8
--- /dev/null
+++ b/digital/io-hub/src/robospierre/path.h
@@ -0,0 +1,78 @@
+#ifndef path_h
+#define path_h
+/* path.h */
+/* 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 "defs.h"
+
+/** This implement a interface similar to the path module, but adapted for the
+ * special grid of Eurobot 2011. See real path modules for interface comments. */
+
+/** Infinite validity for an obstacle. */
+#define PATH_OBSTACLE_VALID_ALWAYS 0xffff
+
+/** Obstacle. */
+struct path_obstacle_t
+{
+ /** Center. */
+ vect_t c;
+ /** Radius. */
+ uint16_t r;
+ /** Validity counter, when this is zero, the obstacle is ignored. */
+ uint16_t valid;
+};
+
+void
+path_init (void);
+
+void
+path_endpoints (vect_t s, vect_t d);
+
+void
+path_escape (uint8_t factor);
+
+void
+path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor,
+ uint16_t valid);
+
+void
+path_decay (void);
+
+void
+path_update (void);
+
+uint8_t
+path_get_next (vect_t *p);
+
+#if AC_PATH_REPORT
+
+/** Report computed path. */
+void
+AC_PATH_REPORT_CALLBACK (vect_t *points, uint8_t len,
+ struct path_obstacle_t *obstacles,
+ uint8_t obstacles_nb);
+
+#endif
+
+#endif /* path_h */
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c
new file mode 100644
index 00000000..cc9e6900
--- /dev/null
+++ b/digital/io-hub/src/robospierre/pawn_sensor.c
@@ -0,0 +1,222 @@
+/* pawn_sensor.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 "defs.h"
+#include "pawn_sensor.h"
+
+#include "asserv.h"
+#include "contact.h"
+#include "logistic.h"
+#include "element.h"
+#include "clamp.h"
+#include "bot.h"
+#include "playground.h"
+#include "codebar.h"
+#include "mimot.h"
+#include "main.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/geometry/distance.h"
+
+/* Handle pawn sensors. When a pawn is detected, it can not be taken
+ * directly, but only once it is inside the robot. */
+
+/** Pawn sensor context. */
+struct pawn_sensor_t
+{
+ /** Is there something in front of the sensor? */
+ uint8_t active;
+ /** If active, supposed position of element. */
+ vect_t active_position;
+};
+
+/** Pawn sensor general context. */
+struct pawn_sensor_general_t
+{
+ /** Activate bumpers. */
+ uint8_t bumper_enabled;
+ /** Last bumped element position. */
+ vect_t last_bumped;
+ /** Bumper triggered, wait until the next one. */
+ uint16_t bump_wait;
+};
+
+/** Global contexts. */
+struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back;
+struct pawn_sensor_general_t pawn_sensor_global;
+
+static uint8_t
+pawn_sensor_get_type (uint8_t direction)
+{
+ uint8_t element_type = codebar_get (direction);
+ if (!element_type)
+ element_type = ELEMENT_PAWN;
+ return element_type;
+}
+
+uint8_t
+pawn_sensor_get (uint8_t direction)
+{
+ struct pawn_sensor_t *ctx;
+ uint8_t contact_value, slot;
+ int16_t dist;
+ /* Check direction. */
+ if (direction == DIRECTION_FORWARD)
+ {
+ ctx = &pawn_sensor_front;
+ contact_value = !IO_GET (CONTACT_FRONT_BOTTOM);
+ slot = CLAMP_SLOT_FRONT_BOTTOM;
+ dist = BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM;
+ }
+ else
+ {
+ ctx = &pawn_sensor_back;
+ contact_value = !IO_GET (CONTACT_BACK_BOTTOM);
+ slot = CLAMP_SLOT_BACK_BOTTOM;
+ dist = BOT_PAWN_BACK_DETECTION_THRESHOLD_MM;
+ }
+ /* Handle contact. */
+ if (contact_value)
+ {
+ if (!logistic_global.slots[slot]
+ && logistic_global.moving_to != slot)
+ {
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ if (ctx->active)
+ {
+ /* In green zone, take it when near enougth from the wall. */
+ if (robot_position.v.x < BOT_GREEN_ELEMENT_DISTANCE_MM + 10
+ || (robot_position.v.x > PG_WIDTH
+ - BOT_GREEN_ELEMENT_DISTANCE_MM - 10))
+ {
+ ctx->active = 0;
+ return pawn_sensor_get_type (direction);
+ }
+ /* Else, take it if near enough from the supposed element
+ * position. */
+ int32_t d = distance_point_point (&ctx->active_position,
+ &robot_position.v);
+ if (d < BOT_PAWN_TAKING_DISTANCE_MM)
+ {
+ ctx->active = 0;
+ return pawn_sensor_get_type (direction);
+ }
+ else if (d > UTILS_ABS (dist))
+ {
+ vect_from_polar_uf016 (&ctx->active_position, dist,
+ robot_position.a);
+ vect_translate (&ctx->active_position, &robot_position.v);
+ }
+ }
+ else
+ {
+ ctx->active = 1;
+ vect_from_polar_uf016 (&ctx->active_position, dist,
+ robot_position.a);
+ vect_translate (&ctx->active_position, &robot_position.v);
+ }
+ }
+ }
+ else
+ {
+ ctx->active = 0;
+ }
+ return 0;
+}
+
+void
+pawn_sensor_bumper (uint8_t bumped, uint16_t dx, uint16_t dy)
+{
+ uint8_t i;
+ if (bumped)
+ {
+ /* Compute pawn position. */
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ vect_t bumped_pawn;
+ bumped_pawn.x = dx;
+ bumped_pawn.y = dy;
+ vect_rotate_uf016 (&bumped_pawn, robot_pos.a);
+ vect_translate (&bumped_pawn, &robot_pos.v);
+ /* Do not count if out of the table. */
+ if (bumped_pawn.x < 400 + BOT_ELEMENT_RADIUS
+ && bumped_pawn.x >= PG_WIDTH - 400 - BOT_ELEMENT_RADIUS
+ && bumped_pawn.y < BOT_ELEMENT_RADIUS
+ && bumped_pawn.y >= PG_WIDTH - BOT_ELEMENT_RADIUS)
+ return;
+ /* Do not count the opponent as a pawn. */
+ for (i = 0; i < main_obstacles_nb; i++)
+ {
+ uint16_t dist = distance_point_point (&bumped_pawn,
+ &main_obstacles_pos[i]);
+ if (dist < 300 + BOT_ELEMENT_RADIUS)
+ return;
+ }
+ /* OK, take it. */
+ pawn_sensor_global.last_bumped = bumped_pawn;
+ fsm_queue_post_event (FSM_EVENT (AI, top_bumper));
+ pawn_sensor_global.bump_wait = 3 * 250;
+ }
+}
+
+void
+pawn_sensor_update (void)
+{
+#define BUMPER_FRONT_LEFT _BV (6)
+#define BUMPER_FRONT_RIGHT _BV (7)
+#define BUMPER_BACK_RIGHT _BV (5)
+#define BUMPER_BACK_LEFT _BV (4)
+ if (pawn_sensor_global.bumper_enabled)
+ {
+ if (pawn_sensor_global.bump_wait)
+ pawn_sensor_global.bump_wait--;
+ else
+ {
+ uint8_t bumpers = mimot_get_input ();
+ pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265);
+ pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265);
+ pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265);
+ pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265);
+ }
+ }
+}
+
+void
+pawn_sensor_bumper_enable (uint8_t enabled)
+{
+ pawn_sensor_global.bumper_enabled = enabled;
+}
+
+vect_t
+pawn_sensor_get_last_bumped (void)
+{
+ return pawn_sensor_global.last_bumped;
+}
+
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h
new file mode 100644
index 00000000..8d6d2651
--- /dev/null
+++ b/digital/io-hub/src/robospierre/pawn_sensor.h
@@ -0,0 +1,45 @@
+#ifndef pawn_sensor_h
+#define pawn_sensor_h
+/* pawn_sensor.h */
+/* 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.
+ *
+ * }}} */
+
+/** Update sensor state and return the element type if an element is ready to
+ * be taken. */
+uint8_t
+pawn_sensor_get (uint8_t direction);
+
+/** Update pawn sensors. */
+void
+pawn_sensor_update (void);
+
+/** Enable bumpers. */
+void
+pawn_sensor_bumper_enable (uint8_t enabled);
+
+/** Return last bumped pawn. */
+vect_t
+pawn_sensor_get_last_bumped (void);
+
+#endif /* pawn_sensor_h */
diff --git a/digital/io-hub/src/robospierre/playground_2011.h b/digital/io-hub/src/robospierre/playground_2011.h
new file mode 100644
index 00000000..a9963e34
--- /dev/null
+++ b/digital/io-hub/src/robospierre/playground_2011.h
@@ -0,0 +1,33 @@
+#ifndef playground_2011_h
+#define playground_2011_h
+/* playground_2011.h */
+/* 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 "playground.h"
+
+/** Width of a green zone. */
+#define PG_GREEN_WIDTH_MM 400
+
+#endif /* playground_2011_h */
diff --git a/digital/io-hub/src/robospierre/radar_defs.c b/digital/io-hub/src/robospierre/radar_defs.c
new file mode 100644
index 00000000..85f015e1
--- /dev/null
+++ b/digital/io-hub/src/robospierre/radar_defs.c
@@ -0,0 +1,46 @@
+/* radar_defs.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 "modules/devices/usdist/usdist.h"
+#include "playground.h"
+
+/** Define radar configuration. */
+struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = {
+ { &usdist_mm[0], { 20, 20 }, G_ANGLE_UF016_DEG (10) },
+ { &usdist_mm[1], { 20, -20 }, G_ANGLE_UF016_DEG (-10) },
+ { &usdist_mm[2], { -20, -20 }, G_ANGLE_UF016_DEG (180 + 10) },
+ { &usdist_mm[3], { -20, 20 }, G_ANGLE_UF016_DEG (180 - 10) },
+};
+
+/** Define exclusion area (considered as invalid point). */
+uint8_t
+radar_valid (vect_t p)
+{
+ return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM
+ && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM;
+}
+
diff --git a/digital/io-hub/src/robospierre/radar_defs.h b/digital/io-hub/src/robospierre/radar_defs.h
new file mode 100644
index 00000000..5f6a2a73
--- /dev/null
+++ b/digital/io-hub/src/robospierre/radar_defs.h
@@ -0,0 +1,41 @@
+#ifndef radar_defs_h
+#define radar_defs_h
+/* radar_defs.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.
+ *
+ * }}} */
+
+#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40
+#define RADAR_OBSTACLE_RADIUS_MM 150
+#define RADAR_STOP_MM 350
+#define RADAR_CLEARANCE_MM 100
+#define RADAR_EPSILON_MM 70
+
+#define RADAR_SENSOR_NB 4
+
+#define RADAR_SENSOR_FRONT_FIRST 0
+#define RADAR_SENSOR_FRONT_NB 2
+#define RADAR_SENSOR_BACK_FIRST 2
+#define RADAR_SENSOR_BACK_NB 2
+
+#endif /* radar_defs_h */
diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c
index 8f4019aa..70055d7d 100644
--- a/digital/io-hub/src/robospierre/simu.host.c
+++ b/digital/io-hub/src/robospierre/simu.host.c
@@ -28,10 +28,25 @@
#include "modules/utils/utils.h"
#include "modules/host/host.h"
#include "modules/host/mex.h"
+#include "modules/adc/adc.h"
+#include "modules/path/path.h"
#include "io.h"
/** AVR registers. */
-uint8_t PINE;
+uint8_t PORTA, DDRA, PINA, PINE, PINF;
+
+/** Message types. */
+uint8_t simu_mex_pos_report;
+uint8_t simu_mex_path;
+
+static void
+simu_adc_handle (void *user, mex_msg_t *msg)
+{
+ uint8_t index;
+ uint16_t value;
+ mex_msg_pop (msg, "BH", &index, &value);
+ adc_values[index] = value;
+}
/** Initialise simulation. */
void
@@ -40,6 +55,10 @@ simu_init (void)
const char *mex_instance;
mex_node_connect ();
mex_instance = host_get_instance ("io-hub0", 0);
+ uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance);
+ mex_node_register (mtype, simu_adc_handle, 0);
+ simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance);
+ simu_mex_path = mex_node_reservef ("%s:path", mex_instance);
}
/** Make a simulation step. */
@@ -62,3 +81,27 @@ timer_wait (void)
return 0;
}
+/** Send computed path. */
+void
+simu_send_path (vect_t *points, uint8_t len,
+ struct path_obstacle_t *obstacles, uint8_t obstacles_nb)
+{
+ int i;
+ mex_msg_t *m;
+ m = mex_msg_new (simu_mex_path);
+ for (i = 0; i < len; i++)
+ mex_msg_push (m, "hh", points[i].x, points[i].y);
+ mex_node_send (m);
+}
+
+void
+simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id)
+{
+ mex_msg_t *m;
+ m = mex_msg_new (simu_mex_pos_report);
+ mex_msg_push (m, "b", id);
+ for (; pos_nb; pos++, pos_nb--)
+ mex_msg_push (m, "hh", pos->x, pos->y);
+ mex_node_send (m);
+}
+
diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h
index 4b461e86..6224e345 100644
--- a/digital/io-hub/src/robospierre/simu.host.h
+++ b/digital/io-hub/src/robospierre/simu.host.h
@@ -24,13 +24,23 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "defs.h"
#ifdef HOST
-extern uint8_t PINE;
+extern uint8_t PORTA, DDRA, PINA, PINE, PINF;
+
+/** Send general purpose positions to indicate computation results.
+ * - pos: array of positions to report.
+ * - pos_nb: number of elements in the array.
+ * - id: identifier so that several unrelated positions could be reported. */
+void
+simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id);
#else /* !defined (HOST) */
+#define simu_send_pos_report(pos, pos_nb, id) ((void) 0)
+
#endif /* !defined (HOST) */
#endif /* simu_host_h */
diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c
new file mode 100644
index 00000000..e200902a
--- /dev/null
+++ b/digital/io-hub/src/robospierre/test_element.c
@@ -0,0 +1,376 @@
+/* test_element.c */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2011 Jérôme Jutteau
+ *
+ * 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 "element.h"
+
+#include "chrono.h"
+
+#include <stdio.h>
+
+#define TEST_PRINT_TYPE_SCORE_PICK 0
+#define TEST_PRINT_TYPE_SCORE_UNLOAD 1
+#define TEST_PRINT_TYPE_ELEMENT 2
+#define TEST_PRINT_TYPE_BONUS_UNLOAD 3
+#define TEST_PRINT_TYPE_BONUS_LOAD 4
+int test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK;
+position_t test_robot_pos_ = {{0, 2100}, 1};
+
+uint8_t PINE;
+
+enum team_color_e team_color;
+
+/** Stubbed chrono. */
+uint32_t test_chrono_ms;
+
+void
+chrono_init (void)
+{
+ test_chrono_ms = CHRONO_MATCH_DURATION_MS;
+}
+
+uint32_t
+chrono_remaining_time (void)
+{
+ return test_chrono_ms;
+}
+
+uint8_t
+test_element_convert (const char *l, uint8_t number)
+{
+ switch (l[0])
+ {
+ case 'A':
+ switch (number)
+ {
+ case 1: return (31);
+ case 3: return (32);
+ case 5: return (33);
+ case 7: return (34);
+ case 9: return (35);
+ case 11: return (36);
+ }
+ case 'B':
+ switch (number)
+ {
+ case 2: return (0);
+ case 4: return (2);
+ case 8: return (3);
+ case 10: return (1);
+ }
+ case 'C':
+ switch (number)
+ {
+ case 1: return (37);
+ case 3: return (38);
+ case 5: return (39);
+ case 7: return (40);
+ case 9: return (41);
+ case 11: return (42);
+ }
+ case 'D':
+ switch (number)
+ {
+ case 0: return (21);
+ case 2: return (4);
+ case 4: return (6);
+ case 8: return (7);
+ case 10: return (5);
+ case 12: return (22);
+ }
+ case 'E':
+ switch (number)
+ {
+ case 1: return (43);
+ case 3: return (44);
+ case 5: return (45);
+ case 7: return (46);
+ case 9: return (47);
+ case 11: return (48);
+ }
+ case 'F':
+ switch (number)
+ {
+ case 0: return (23);
+ case 12: return (24);
+ }
+ case 'G':
+ switch (number)
+ {
+ case 2: return (8);
+ case 4: return (10);
+ case 6: return (20);
+ case 8: return (11);
+ case 10: return (9);
+ }
+ case 'H':
+ switch (number)
+ {
+ case 0: return (25);
+ case 1: return (49);
+ case 3: return (50);
+ case 5: return (51);
+ case 7: return (52);
+ case 9: return (53);
+ case 11: return (54);
+ case 12: return (26);
+ }
+ case 'I':
+ switch (number) /*12*/
+ {
+ case 2: return (12);
+ case 4: return (14);
+ case 8: return (15);
+ case 10: return (13);
+ }
+ case 'J':
+ switch (number)
+ {
+ case 0: return (27);
+ case 12: return (28);
+ }
+ case 'K':
+ switch (number)
+ {
+ case 1: return (55);
+ case 3: return (56);
+ case 5: return (57);
+ case 7: return (58);
+ case 9: return (59);
+ case 11: return (60);
+ }
+ case 'L':
+ switch (number)
+ {
+ case 2: return (16);
+ case 4: return (18);
+ case 8: return (19);
+ case 10: return (17);
+ }
+ case 'M':
+ switch (number)
+ {
+ case 0: return (29);
+ case 1: return (61);
+ case 3: return (62);
+ case 5: return (63);
+ case 7: return (64);
+ case 9: return (65);
+ case 11: return (66);
+ case 12: return (30);
+ }
+ }
+ return 0;
+}
+
+long int
+test_element_get_score (uint8_t element_id)
+{
+ if (test_print_type_ == TEST_PRINT_TYPE_SCORE_PICK)
+ return (long int) element_score (test_robot_pos_, element_id);
+ else if (test_print_type_ == TEST_PRINT_TYPE_SCORE_UNLOAD)
+ return (long int) element_unload_score (test_robot_pos_, element_id);
+ else if (test_print_type_ == TEST_PRINT_TYPE_BONUS_LOAD)
+ {
+ element_t e = element_get (element_id);
+ return (long int) e.bonus_load;
+ }
+ else if (test_print_type_ == TEST_PRINT_TYPE_BONUS_UNLOAD)
+ {
+ element_t e = element_get (element_id);
+ return (long int) e.bonus_unload;
+ }
+ else
+ return (long int) element_table[element_id].type;
+}
+
+#define p(l, n) (test_element_print (#l, n))
+long int
+test_element_print (const char *l, uint8_t number)
+{
+ return test_element_get_score (test_element_convert (l, number));
+}
+
+void
+test_element_print_table ()
+{
+ printf ("parameters:\n");
+ printf ("- robot_pos = (%u, %u)\n", test_robot_pos_.v.x, test_robot_pos_.v.y);
+ printf ("- side = %u\n", team_color);
+ printf (" _____0_________1_____2____3____4____5____6____7____8____9___10___11_________12_____ \n");
+ printf (" | | | | | | | | |\n");
+ printf (" | | | | | | | | |\n");
+ printf ("A| Red | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | Blue |\n", p (A, 1), p (A, 3), p (A, 5), p(A, 7), p (A, 9), p (A, 11));
+ printf (" | | | | | | | | |\n");
+ printf ("B| |--------%5ld-----%5ld------+--------%5ld-----%5ld------| |\n", p (B, 2), p (B, 4), p (B, 8), p (B, 10));
+ printf (" |-----------+ | | | | | +-----------|\n");
+ printf (" | | | [B] | | | [R] | | |\n");
+ printf ("C| | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | |\n", p (C, 1), p (C, 3), p (C, 5), p (C, 7), p(C, 9), p (C, 11));
+ printf (" | | | | | | | | |\n");
+ printf ("D| %5ld |--------%5ld-----%5ld------+--------%5ld-----%5ld------| %5ld |\n", p (D, 0), p (D, 2), p (D, 4), p (D, 8), p (D, 10), p (D, 12));
+ printf (" | | | | | | | | |\n");
+ printf (" | | | | | | | | |\n");
+ printf ("E| | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | |\n", p (E, 1), p (E, 3), p (E, 5), p (E, 7), p(E, 9), p(E, 11));
+ printf ("F| %5ld | | | | | | | %5ld |\n", p (F, 0), p(F, 12));
+ printf ("G| |--------%5ld-----%5ld-----%5ld-----%5ld-----%5ld------| |\n", p (G, 2), p (G, 4), p(G, 6), p (G, 8), p (G, 10));
+ printf (" | | | | | | | | |\n");
+ printf (" | | | [B] | | | [R] | | |\n");
+ printf ("H| %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld |\n", p(H, 0), p (H, 1), p (H, 3), p (H, 5), p (H, 7), p(H, 9), p(H, 11), p(H, 12));
+ printf (" | | | | | | | | |\n");
+ printf ("I| |--------%5ld-----%5ld------+--------%5ld-----%5ld------| |\n", p (I, 2), p (I, 4), p (I, 8), p (I, 10));
+ printf (" | | | | | | | | |\n");
+ printf ("J| %5ld | | | | | | | %5ld |\n", p (J, 0), p(J, 12));
+ printf ("K| | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | |\n", p (K, 1), p (K, 3), p (K, 5), p (K, 7), p(K, 9), p(K, 11));
+ printf (" | | | | | | | | |\n");
+ printf ("L| |--------%5ld-----%5ld------+--------%5ld-----%5ld------| |\n", p (L, 2), p (L, 4), p (L, 8), p (L, 10));
+ printf (" | | | | | | | | |\n");
+ printf ("M| %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld |\n", p(M, 0), p (M, 1), p (M, 3), p (M, 5), p (M, 7), p(M, 9), p(M, 11), p(M, 12));
+ printf (" | |###################| [R] | [B] |###################| |\n");
+ printf (" | |###################| | |###################| |\n");
+ printf (" +-----------|###################+---------+---------+###################+-----------+\n");
+}
+
+int main ()
+{
+ int exit = 0;
+ char cmd;
+ int x;
+ char y[10], z[10];
+ int type = 0;
+ chrono_init ();
+
+ while (!exit)
+ {
+ element_init ();
+ printf ("\ncommands:\n");
+ printf ("s: print scores, ");
+ printf ("a: print load bonus, ");
+ printf ("p: print unload scores, ");
+ printf ("z: print unload bonus, ");
+ printf ("e: print elements, ");
+ printf ("t: set match time, ");
+ printf ("c: set robot side, ");
+ printf ("r: set robot position, ");
+ printf ("i: pick up an element, ");
+ printf ("n: indicate there is no element, ");
+ printf ("m: id of nearest element, ");
+ printf ("g: get position and angle of desired element,");
+ printf ("u: call I like green,");
+ printf ("q: quit\n");
+ printf ("your choice: ");
+ fflush (stdin);
+ scanf ("%c", &cmd);
+ switch (cmd)
+ {
+ case 's':
+ test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK;
+ test_element_print_table ();
+ break;
+ case 'a':
+ test_print_type_ = TEST_PRINT_TYPE_BONUS_LOAD;
+ test_element_print_table ();
+ break;
+ case 'p':
+ test_print_type_ = TEST_PRINT_TYPE_SCORE_UNLOAD;
+ test_element_print_table ();
+ break;
+ case 'z':
+ test_print_type_ = TEST_PRINT_TYPE_BONUS_UNLOAD;
+ test_element_print_table ();
+ break;
+ case 'e':
+ test_print_type_ = TEST_PRINT_TYPE_ELEMENT;
+ test_element_print_table ();
+ break;
+ case 't':
+ printf ("set match time: ");
+ scanf ("%i", &test_chrono_ms);
+ break;
+ case 'c':
+ if (team_color == TEAM_COLOR_RIGHT)
+ team_color = TEAM_COLOR_LEFT;
+ else
+ team_color = TEAM_COLOR_RIGHT;
+ team_color ^= TEAM_COLOR_RIGHT;
+ printf ("Side is now %u\n", team_color);
+ break;
+ case 'r':
+ printf ("set robot position:\n");
+ printf ("number: ");
+ scanf ("%i", &x);
+ printf ("letter: ");
+ scanf ("%s", y);
+ test_robot_pos_.v = element_table[test_element_convert (y, x)].pos;
+ break;
+ case 'i':
+ printf ("pick up an element:\n");
+ printf ("number: ");
+ scanf ("%i", &x);
+ printf ("letter: ");
+ scanf ("%s", y);
+ printf ("type: (P=Pawn Q=Queen K=King) ");
+ fflush (stdin);
+ scanf ("%s", z);
+ if (z[0] == 'P')
+ type = ELEMENT_PAWN;
+ else if (z[0] == 'Q')
+ type = ELEMENT_QUEEN;
+ else if (z[0] == 'K')
+ type = ELEMENT_KING;
+ else
+ printf ("Unknown !\n");
+ element_taken (test_element_convert (y, x), type);
+ break;
+ case 'n':
+ printf ("indicate there no element\n");
+ printf ("number: ");
+ scanf ("%i", &x);
+ printf ("letter: ");
+ scanf ("%s", y);
+ element_not_here (test_element_convert (y, x));
+ break;
+ case 'm':
+ printf ("nearest element id from robot: %u\n",
+ element_nearest_element_id (test_robot_pos_));
+ break;
+ case 'g':
+ printf ("element id: ");
+ scanf ("%i", &x);
+ vect_t pos = element_get_pos (x);
+ printf ("x: %u y: %u\n", pos.x, pos.y);
+ break;
+ case 'u':
+ printf ("call I like green ...");
+ element_i_like_green ();
+ break;
+ case 'q':
+ exit = 1;
+ break;
+ }
+ fflush (stdin);
+ scanf ("%c", &cmd);
+ }
+ return 0;
+}
+
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
new file mode 100644
index 00000000..93fb447f
--- /dev/null
+++ b/digital/io-hub/src/robospierre/top.c
@@ -0,0 +1,476 @@
+/* top.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 "io.h"
+
+#include "playground_2011.h"
+#include "asserv.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+
+#include "logistic.h"
+#include "move.h"
+#include "chrono.h"
+#include "pawn_sensor.h"
+#include "contact.h"
+
+/*
+ * Here is the top FSM. This FSM is suppose to give life to the robot with an
+ * impression of intelligence... Well...
+ */
+
+#define TOP_PAWN_TIME 45000l
+
+FSM_INIT
+
+FSM_STATES (
+ /* Initial state. */
+ TOP_START,
+ /* Going out of start area. */
+ TOP_GOING_OUT1,
+ /* Problem going out, wait before retry. */
+ TOP_GOING_OUT1_BLOCK_WAIT,
+ /* Going out, first pawn emplacement. */
+ TOP_GOING_OUT2,
+ /* Problem going out, wait before retry. */
+ TOP_GOING_OUT2_BLOCK_WAIT,
+
+ TOP_GOING_TO_DROP,
+ TOP_GOING_TO_ELEMENT,
+ /* Waiting clamp has finished its work. */
+ TOP_WAITING_CLAMP,
+ /* Unblocking: waiting a little bit. */
+ TOP_UNBLOCKING_SHAKE_WAIT,
+ /* Unblocking: shaking. */
+ TOP_UNBLOCKING_SHAKE,
+ /* Waiting construction is ready to drop. */
+ TOP_WAITING_READY,
+ /* Dropping, opening the doors. */
+ TOP_DROP_DROPPING,
+ /* Dropping, clearing so that doors can be closed. */
+ TOP_DROP_CLEARING)
+
+FSM_EVENTS (
+ /* Bumpers have seen something. */
+ top_bumper)
+
+FSM_START_WITH (TOP_START)
+
+/** Top context. */
+struct top_t
+{
+ /** Target element. */
+ uint8_t target_element_id;
+ /** Chaos counter. */
+ uint8_t chaos;
+ /** Broken clamp. */
+ uint8_t broken;
+ /** Saved, direction when picking element. */
+ uint8_t go_to_element_direction;
+ /** Return to our green zone again. */
+ uint8_t green_again;
+};
+
+/** Global context. */
+struct top_t top_global;
+#define ctx top_global
+
+FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1)
+{
+ element_init ();
+ ctx.green_again = 3;
+ asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
+ PG_Y (PG_LENGTH - 200), 0);
+ return FSM_NEXT (TOP_START, init_start_round);
+}
+
+FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2)
+{
+ asserv_goto (PG_X (1500 - 2 * 350), PG_Y (PG_LENGTH - 350), 0);
+ return FSM_NEXT (TOP_GOING_OUT1, robot_move_success);
+}
+
+FSM_TRANS (TOP_GOING_OUT1, robot_move_failure, TOP_GOING_OUT1_BLOCK_WAIT)
+{
+ return FSM_NEXT (TOP_GOING_OUT1, robot_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1)
+{
+ asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
+ PG_Y (PG_LENGTH - 200 - (++ctx.chaos % 4) * 10), 0);
+ return FSM_NEXT_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT);
+}
+
+static uint8_t
+top_prepare_level (void)
+{
+ if (ctx.broken)
+ return 3;
+ else if (logistic_global.need_prepare
+ || chrono_remaining_time () < TOP_PAWN_TIME)
+ return 2;
+ else
+ return 1;
+}
+
+static void
+top_go_this_element (vect_t pos, int16_t shorten)
+{
+ ctx.go_to_element_direction = logistic_global.collect_direction;
+ uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
+ ? 0 : ASSERV_BACKWARD;
+ move_start_noangle (pos, backward, shorten);
+}
+
+static uint8_t
+top_go_element (void)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ ctx.target_element_id = element_best (robot_pos);
+ element_t e = element_get (ctx.target_element_id);
+ if (!ctx.broken)
+ {
+ if (e.attr & ELEMENT_GREEN)
+ {
+ logistic_global.prepare = 0;
+ pawn_sensor_bumper_enable (0);
+ }
+ else
+ {
+ logistic_global.prepare = top_prepare_level ();
+ pawn_sensor_bumper_enable (1);
+ }
+ }
+ vect_t element_pos = element_get_pos (ctx.target_element_id);
+ top_go_this_element (element_pos, 0);
+ return 1;
+}
+
+static uint8_t
+top_go_drop (void)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ ctx.target_element_id = element_unload_best (robot_pos);
+ position_t drop_pos;
+ drop_pos.v = element_get_pos (ctx.target_element_id);
+ if (!ctx.broken)
+ logistic_global.prepare = top_prepare_level ();
+ pawn_sensor_bumper_enable (0);
+ uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
+ ? 0 : ASSERV_BACKWARD;
+ /* Go above or below the drop point. */
+ if (drop_pos.v.y < 350)
+ {
+ /* Protected zone. */
+ drop_pos.v.x = PG_WIDTH / 2;
+ drop_pos.v.y = 350;
+ drop_pos.a = PG_A_DEG (45);
+ }
+ else if (drop_pos.v.y > PG_LENGTH / 2)
+ {
+ drop_pos.v.y -= 350 / 2;
+ drop_pos.a = PG_A_DEG (-90);
+ }
+ else
+ {
+ drop_pos.v.y += 350 / 2;
+ drop_pos.a = PG_A_DEG (90);
+ }
+ if (logistic_global.collect_direction == DIRECTION_BACKWARD)
+ drop_pos.a += POSITION_A_DEG (180);
+ /* Go! */
+ move_start (drop_pos, backward);
+ return 0;
+}
+
+static uint8_t
+top_decision (void)
+{
+ /* If we can make a tower. */
+ if (logistic_global.construct_possible == 1
+ || ((ctx.broken || chrono_remaining_time () < TOP_PAWN_TIME)
+ && logistic_global.construct_possible == 2))
+ return top_go_drop ();
+ if (logistic_global.need_prepare)
+ {
+ clamp_prepare (top_prepare_level ());
+ return top_go_drop ();
+ }
+ else
+ return top_go_element ();
+}
+
+FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop);
+ case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element);
+ }
+}
+
+FSM_TRANS (TOP_GOING_OUT2, robot_move_failure, TOP_GOING_OUT2_BLOCK_WAIT)
+{
+ return FSM_NEXT (TOP_GOING_OUT2, robot_move_failure);
+}
+
+FSM_TRANS_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, 250,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ if (clamp_working ())
+ return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, clamp_working);
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, drop);
+ case 1: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, element);
+ }
+}
+
+FSM_TRANS (TOP_GOING_TO_DROP, move_success,
+ ready, TOP_DROP_DROPPING,
+ wait_clamp, TOP_WAITING_READY)
+{
+ if (logistic_global.ready)
+ {
+ if (clamp_drop (logistic_global.collect_direction))
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready);
+ else
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
+ }
+ else
+ {
+ clamp_prepare (top_prepare_level ());
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
+ }
+}
+
+FSM_TRANS (TOP_GOING_TO_DROP, move_failure,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ if (ctx.target_element_id != 0xff)
+ element_failure (ctx.target_element_id);
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, clamp_working);
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, drop);
+ case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, element);
+ }
+}
+
+FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP)
+{
+ move_stop ();
+ return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working);
+}
+
+FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ if (ctx.target_element_id != 0xff)
+ element_failure (ctx.target_element_id); /* Do not take this one again. */
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
+ if (ctx.green_again)
+ {
+ ctx.green_again--;
+ if (ctx.green_again == 0)
+ element_no_more_green ();
+ }
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop);
+ case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element);
+ }
+}
+
+FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
+ clamp_working, TOP_WAITING_CLAMP,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ if (ctx.target_element_id != 0xff)
+ element_failure (ctx.target_element_id);
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working);
+ if (ctx.green_again)
+ {
+ ctx.green_again--;
+ if (ctx.green_again == 0)
+ element_no_more_green ();
+ }
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop);
+ case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, element);
+ }
+}
+
+FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP)
+{
+ move_stop ();
+ if (ctx.green_again)
+ {
+ ctx.green_again--;
+ if (ctx.green_again == 0)
+ element_no_more_green ();
+ }
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working);
+}
+
+FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT)
+{
+ if (!ctx.broken)
+ logistic_global.prepare = top_prepare_level ();
+ move_stop ();
+ ctx.target_element_id = 0xff;
+ top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50);
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper);
+}
+
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop);
+ case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element);
+ }
+}
+
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)
+{
+ return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked);
+}
+
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_taken, TOP_WAITING_CLAMP)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ if (robot_pos.v.x < 400 || robot_pos.v.x > PG_WIDTH - 400)
+ asserv_move_linearly (ctx.go_to_element_direction
+ == DIRECTION_FORWARD ? -50 : 50);
+ return FSM_NEXT (TOP_WAITING_CLAMP, clamp_taken);
+}
+
+FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250,
+ try_again, TOP_UNBLOCKING_SHAKE,
+ tryout, TOP_WAITING_CLAMP)
+{
+ if (ctx.chaos < 4)
+ {
+ int16_t dist = logistic_global.collect_direction
+ == DIRECTION_FORWARD ? 100 : -100;
+ asserv_move_linearly (++ctx.chaos % 2 ? -dist : dist);
+ return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, try_again);
+ }
+ else
+ {
+ ctx.broken = 1;
+ clamp_prepare (3);
+ return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, tryout);
+ }
+}
+
+FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_success, TOP_WAITING_CLAMP)
+{
+ clamp_prepare (0);
+ return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_success);
+}
+
+FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure,
+ TOP_UNBLOCKING_SHAKE_WAIT)
+{
+ return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure);
+}
+
+FSM_TRANS (TOP_WAITING_READY, clamp_done,
+ drop, TOP_DROP_DROPPING,
+ not_ready, TOP_WAITING_READY)
+{
+ if (clamp_drop (logistic_global.collect_direction))
+ return FSM_NEXT (TOP_WAITING_READY, clamp_done, drop);
+ else
+ return FSM_NEXT (TOP_WAITING_READY, clamp_done, not_ready);
+}
+
+FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)
+{
+ return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked);
+}
+
+FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING)
+{
+ if (ctx.target_element_id != 0xff)
+ element_down (ctx.target_element_id,
+ logistic_drop_element_type
+ (logistic_global.collect_direction));
+ if (IO_GET (CONTACT_STRAT))
+ element_i_like_green ();
+ asserv_move_linearly (logistic_global.collect_direction
+ == DIRECTION_FORWARD ? 150 : -150);
+ return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting);
+}
+
+FSM_TRANS (TOP_DROP_CLEARING, robot_move_success,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ clamp_drop_clear ();
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop);
+ case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, element);
+ }
+}
+
+FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ clamp_drop_clear ();
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop);
+ case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, element);
+ }
+}
+