summaryrefslogtreecommitdiff
path: root/digital/beacon/src/Bitcloud_stack/lib/MakerulesBc_All_Sec_Atmega1281_Rf231_Gcc
blob: 9af06bb57cd58b7f1f46ed838eaa348abd7bfe4c (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
#----------------------------------------------
#User application makerules - should be included into user application Makefile
#----------------------------------------------

include $(COMPONENTS_PATH)/../lib/Makerules_Atmega1281_Gcc


#-Compiler flags-------------------------------
CFLAGS =  -Os -std=gnu99  -pipe -c -W -Wall    -ffunction-sections -mmcu=atmega1281 -mcall-prologues -fshort-enums --param inline-call-cost=2 -DATMEGA1281 -DAT86RF231 -DNONE_OS -D_IEEE_ZIGBEE_COMPLIANCE_ -D_SYS_MAC_PHY_HWD_TASK_ -D_SYS_HAL_TASK_ -D_SYS_MAC_HWI_TASK_ -D_SYS_BSP_TASK_ -D_SYS_APL_TASK_ -D_SYS_NWK_TASK_ -D_SYS_APS_TASK_ -D_SYS_SSP_TASK_ -D_SYS_TC_TASK_ -D_SYS_ZDO_TASK_ -D_SECURITY_         -D_NWK_ALLOCATOR_     -D_MAC_HW_AES_       -D_TRUST_CENTRE_ -D_UNIVERSAL_TC_EXT_ADDR_ -D_COORDINATOR_ -D_ROUTER_ -D_ENDDEVICE_ -D_FFD_ -D_NWK_FAST_ROUTE_DISCOVERY_ -D_NWK_NONSTANDARD_BEACON_FILTER_ -D_NWK_GROUP_ -D_GROUP_TABLE_ -D_NWK_CHECK_OUT_BROADCAST_ -D_NWK_ROUTING_OPTIMIZATION_=3 -D_NWK_STOCHASTIC_ADDRESSING_ -D_RESOLVE_ADDR_CONFLICT_ -D_NWK_MESH_ROUTING_ -D_APS_FRAGMENTATION_ -D_APS_MULTICAST_ -D_GROUP_TABLE_ -D_BINDING_ -D_COMMISSIONING_ -D_POWER_FAILURE_ -D_NWK_PASSIVE_ACK_
CFLAGS += $(BOARDCFLAGS)
#-Libraries names------------------------------
CS_LIB    = ConfigServer
PDS_LIB   = PersistDataServer

#-Stack components paths-----------------------
HAL_HWD_COMMON_PATH = $(COMPONENTS_PATH)/./HAL/avr/atmega1281/common
HAL_MAC_API_PATH = $(COMPONENTS_PATH)/./HAL/avr/atmega1281/zigBit
HAL_PATH     = $(COMPONENTS_PATH)/./HAL
MAC_PHY_PATH = $(COMPONENTS_PATH)/./MAC_PHY
MAC_ENV_PATH = $(COMPONENTS_PATH)/./MAC_PHY/MAC_ENV
MAC_HWD_PATH = $(COMPONENTS_PATH)/./MAC_PHY/MAC_HWD_PHY
MAC_HWI_PATH = $(COMPONENTS_PATH)/./MAC_PHY/MAC_HWI
NWK_PATH     = $(COMPONENTS_PATH)/./NWK
APS_PATH     = $(COMPONENTS_PATH)/./APS
ZDO_PATH     = $(COMPONENTS_PATH)/./ZDO
SSP_PATH     = $(COMPONENTS_PATH)/./Security/ServiceProvider
TC_PATH      = $(COMPONENTS_PATH)/./Security/TrustCentre
CS_PATH      = $(COMPONENTS_PATH)/./ConfigServer
PDS_PATH     = $(COMPONENTS_PATH)/./PersistDataServer
BSP_PATH     = $(COMPONENTS_PATH)/./BSP
DRIVERS_PATH = $(COMPONENTS_PATH)/./HAL/drivers
> 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981
/* clamp.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 "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 "logistic.h"
#include "pawn_sensor.h"

/*
 * 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 (
	    /* 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_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_START)
FSM_START_WITH (CLAMP_MOVE_IDLE)

/** Clamp context. */
struct clamp_t
{
    /** True if clamp is working. */
    uint8_t working;
    /** Current position. */
    uint8_t pos_current;
    /** 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. */
struct clamp_t clamp_global;
#define ctx clamp_global

/** Clamp positions. */
static const uint16_t clamp_pos[][2] = {
      { BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP,
	BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP },
      { BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP,
	BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP },
      { BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP,
	BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP },
      { BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP,
	BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP },
      { BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP,
	BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP },
      { BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_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. */
static void
clamp_route (void)
{
    uint8_t pos_new;
    uint8_t pos_current = ctx.pos_current;
    uint8_t pos_request = ctx.pos_request;
    /* Compute new position. */
    if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current))
      {
	if (!CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request))
	    pos_new = CLAMP_BAY_FRONT_LEAVE;
	else
	    pos_new = pos_request;
      }
    else if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_current))
      {
	if (!CLAMP_IS_SLOT_IN_BACK_BAY (pos_request))
	    pos_new = CLAMP_BAY_BACK_LEAVE;
	else
	    pos_new = pos_request;
      }
    else if (pos_current == CLAMP_SLOT_SIDE)
      {
	pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE;
      }
    else if (pos_current == CLAMP_BAY_FRONT_LEAVE)
      {
	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_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_FRONT_MIDDLE;
      }
    else
      {
	assert (pos_current == CLAMP_BAY_SIDE_ENTER_LEAVE);
	if (pos_request == CLAMP_SLOT_SIDE)
	    pos_new = pos_request;
	else if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request))
	    pos_new = CLAMP_SLOT_FRONT_MIDDLE;
	else
	    pos_new = CLAMP_SLOT_BACK_MIDDLE;
      }
    /* Run motors. */
    mimot_move_motor0_absolute (clamp_pos[pos_new][0],
				BOT_CLAMP_ELEVATION_SPEED);
    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;
}

static void
clamp_taken_pawn (uint8_t element_type)
{
    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);
    clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1);
    clamp_door (CLAMP_SLOT_FRONT_TOP, 1);
    clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1);
    clamp_door (CLAMP_SLOT_BACK_TOP, 1);
    mimot_motor0_free ();
    mimot_motor1_free ();
    logistic_dump ();
    /* 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