From 366eeaccdca0ce784415a7b1bbc7e75f7c889af7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 01:36:57 +0200 Subject: digital/io-hub: add first element handling in clamp --- digital/io-hub/src/robospierre/Makefile | 4 +- digital/io-hub/src/robospierre/clamp.c | 146 +++++++++++++++++++- digital/io-hub/src/robospierre/clamp.h | 5 + digital/io-hub/src/robospierre/logistic.c | 212 ++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 68 ++++++++++ digital/io-hub/src/robospierre/main.c | 23 ++++ 6 files changed, 453 insertions(+), 5 deletions(-) create mode 100644 digital/io-hub/src/robospierre/logistic.c create mode 100644 digital/io-hub/src/robospierre/logistic.h (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 21411631..20e9dee0 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -4,8 +4,8 @@ BASE = ../../../avr PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ - clamp.c \ - fsm.host.c fsm_AI_gen.avr.c \ + clamp.c logistic.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 \ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 5d9533fa..1ee020e9 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -25,16 +25,42 @@ #include "common.h" #include "clamp.h" -#define FSM_NAME AI -#include "fsm.h" - #include "mimot.h" #include "pwm.h" +#include "contact.h" #include "bot.h" +#include "element.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "logistic.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_INIT FSM_STATES ( + /* Initial state, to be complete with full initialisation. */ + CLAMP_START, + /* 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, + /* Waiting movement order. */ CLAMP_MOVE_IDLE, /* Moving to a final or intermediary position. */ @@ -53,8 +79,14 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( + /* Here for the moment, to be moved later. */ + start, + /* New element inside bottom slot. */ + clamp_new_element, /* Order to move the clamp. */ clamp_move, + /* Clamp movement success. */ + clamp_move_success, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, /* Elevation motor failure. */ @@ -62,6 +94,7 @@ FSM_EVENTS ( /* Rotation motor failure. */ clamp_rotation_failure) +FSM_START_WITH (CLAMP_START) FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ @@ -73,6 +106,10 @@ struct clamp_t uint8_t pos_request; /** Element moving destination. */ uint8_t moving_to; + /** Position of a new element. */ + uint8_t pos_new; + /** New element kind. */ + uint8_t new_element; }; /** Global context. */ @@ -123,16 +160,51 @@ clamp_move (uint8_t 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; FSM_HANDLE (AI, clamp_move); } +void +clamp_new_element (uint8_t pos, uint8_t element) +{ + assert (pos == CLAMP_SLOT_FRONT_BOTTOM || pos == CLAMP_SLOT_BACK_BOTTOM); + ctx.pos_new = pos; + ctx.new_element = element; + FSM_HANDLE (AI, clamp_new_element); +} + +uint8_t +clamp_handle_event (void) +{ + if (FSM_CAN_HANDLE (AI, clamp_new_element)) + { + /* XXX: temporary hack. */ + uint8_t element = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + if (!IO_GET (CONTACT_FRONT_BOTTOM) + && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM]) + { + clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element); + return 1; + } + if (!IO_GET (CONTACT_BACK_BOTTOM) + && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM]) + { + clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element); + return 1; + } + } + return 0; +} + /** Find next position and start motors. */ static void clamp_route (void) @@ -192,6 +264,72 @@ clamp_route (void) ctx.pos_current = pos_new; } +/* CLAMP FSM */ + +FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE) +{ + clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + return FSM_NEXT (CLAMP_START, start); +} + +FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) +{ + return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); +} + +FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) +{ + pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE); + return FSM_NEXT (CLAMP_IDLE, clamp_new_element); +} + +FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + done, CLAMP_IDLE) +{ + logistic_element_new (ctx.pos_new, ctx.new_element); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); + } + else + 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, + done, CLAMP_IDLE) +{ + logistic_element_move_done (); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + done); +} + +/* CLAMP_MOVE FSM */ + FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, move, CLAMP_MOVE_ROUTING, move_element, CLAMP_MOVE_SRC_ROUTING, @@ -223,6 +361,7 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, { if (ctx.pos_current == ctx.pos_request) { + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success)); return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, done); } @@ -318,6 +457,7 @@ FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, 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 7b488f92..8c5f5c4c 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -62,4 +62,9 @@ clamp_move (uint8_t pos); void clamp_move_element (uint8_t from, uint8_t to); +/** 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/logistic.c b/digital/io-hub/src/robospierre/logistic.c new file mode 100644 index 00000000..534f1c46 --- /dev/null +++ b/digital/io-hub/src/robospierre/logistic.c @@ -0,0 +1,212 @@ +/* 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 "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"); +#endif +} + +/** Examine current state and take a decision. */ +static void +logistic_decision (void) +{ + uint8_t i; + /* If currently moving, do not take decision. */ + if (ctx.moving_from != CLAMP_SLOT_NB) + return; + /* Determine collect_direction. */ + uint8_t front_head = 0, back_head = 0, + front_element = 0, back_element = 0; + uint8_t collect_direction; + for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_FRONT_TOP; i++) + { + if (ctx.slots[i]) + { + front_element++; + if (ELEMENT_IS_HEAD (ctx.slots[i])) + front_head++; + } + } + for (i = CLAMP_SLOT_BACK_BOTTOM; i <= CLAMP_SLOT_BACK_TOP; i++) + { + if (ctx.slots[i]) + { + back_element++; + if (ELEMENT_IS_HEAD (ctx.slots[i])) + back_head++; + } + } + if (front_head < back_head) + collect_direction = DIRECTION_FORWARD; + else if (front_head > back_head) + collect_direction = DIRECTION_BACKWARD; + else if (front_head) + { + if (front_element < back_element) + collect_direction = DIRECTION_FORWARD; + else if (front_element > back_element) + collect_direction = DIRECTION_BACKWARD; + else + collect_direction = ctx.collect_direction; + } + else + collect_direction = ctx.collect_direction; + ctx.collect_direction = collect_direction; + /* Now use this direction. */ + uint8_t collect_bay, storage_bay; + uint8_t collect_bay_head, storage_bay_head; + if (collect_direction == DIRECTION_FORWARD) + { + collect_bay = CLAMP_SLOT_FRONT_BOTTOM; + storage_bay = CLAMP_SLOT_BACK_BOTTOM; + collect_bay_head = front_head; + storage_bay_head = back_head; + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + } + else + { + collect_bay = CLAMP_SLOT_BACK_BOTTOM; + storage_bay = CLAMP_SLOT_FRONT_BOTTOM; + collect_bay_head = back_head; + storage_bay_head = front_head; + ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; + } + /* Find a destination for an element move. */ + uint8_t moving_to = CLAMP_SLOT_NB; + uint8_t moving_from = CLAMP_SLOT_NB; + if (!ctx.slots[collect_bay + 1]) + { + /* Movements in collect bay possible. */ + if (ELEMENT_IS_HEAD (ctx.slots[collect_bay + 0])) + { + moving_to = collect_bay + 2; + moving_from = collect_bay + 0; + } + } + if (moving_to == CLAMP_SLOT_NB && !ctx.slots[storage_bay + 1]) + { + /* No movement yet and movements in storage bay possible. */ + if (ELEMENT_IS_HEAD (ctx.slots[storage_bay + 0])) + { + moving_to = storage_bay + 2; + moving_from = storage_bay + 0; + } + else if (storage_bay_head) + { + if (!ctx.slots[storage_bay + 0]) + moving_to = storage_bay + 0; + else if (!ctx.slots[storage_bay + 1]) + moving_to = storage_bay + 1; + } + } + if (moving_to == CLAMP_SLOT_NB && !ctx.slots[CLAMP_SLOT_SIDE]) + { + /* No movement yet, store in side slot. */ + moving_to = CLAMP_SLOT_SIDE; + } + /* Find a source if available. */ + if (moving_to != CLAMP_SLOT_NB && moving_from == CLAMP_SLOT_NB) + { + if (ctx.slots[collect_bay + 0]) + moving_from = collect_bay + 0; + else if (ctx.slots[CLAMP_SLOT_SIDE]) + moving_from = CLAMP_SLOT_SIDE; + } + /* Ask for movement. */ + if (moving_from != CLAMP_SLOT_NB) + { + ctx.moving_from = moving_from; + ctx.moving_to = moving_to; + } + logistic_debug_dump (); +} + +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; +} + +void +logistic_update (void) +{ +} + +void +logistic_element_new (uint8_t pos, uint8_t element) +{ + assert (pos < CLAMP_SLOT_NB); + assert (!ctx.slots[pos]); + ctx.slots[pos] = element; + logistic_decision (); +} + +void +logistic_element_move_done (void) +{ + assert (!ctx.slots[ctx.moving_to]); + 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 (); +} + diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h new file mode 100644 index 00000000..75b49d53 --- /dev/null +++ b/digital/io-hub/src/robospierre/logistic.h @@ -0,0 +1,68 @@ +#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" + +/** 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; +}; + +/** Global context. */ +extern struct logistic_t logistic_global; + +/** Initialise module. */ +void +logistic_init (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); + +/** To be called when a element movement is done. */ +void +logistic_element_move_done (void); + +#endif /* logistic_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 13bad26c..ffb05a99 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -44,8 +44,10 @@ #ifdef HOST # include #endif +#include "fsm_queue.h" #include "clamp.h" +#include "logistic.h" #include "bot.h" @@ -78,6 +80,8 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + /* AI modules. */ + logistic_init (); /* Initialization done. */ proto_send0 ('z'); } @@ -107,6 +111,23 @@ main_event_to_fsm (void) FSM_HANDLE_E (AI, clamp_elevation_failure); else if (mimot_motor1_status == failure) FSM_HANDLE_E (AI, clamp_rotation_failure); + /* Clamp specific events. */ + if (clamp_handle_event ()) + return; + /* Jack, XXX to be changed! */ + if (!contact_get_jack ()) + FSM_HANDLE_E (AI, start); + /* 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); + } + } /** Main (and infinite) loop. */ @@ -132,6 +153,8 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + /* Update AI modules. */ + logistic_update (); /* Only manage events if slaves are synchronised. */ if (twi_master_sync ()) main_event_to_fsm (); -- cgit v1.2.3