From 2376aeb614d69d7050209f858818bfa7d36be398 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 2 May 2011 23:32:51 +0200 Subject: digital/io: adapt FSM to new FSM system (AngFSM) --- digital/io/src/ai_loader_cb.c | 636 ------------------------------------------ 1 file changed, 636 deletions(-) delete mode 100644 digital/io/src/ai_loader_cb.c (limited to 'digital/io/src/ai_loader_cb.c') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c deleted file mode 100644 index fdb628a9..00000000 --- a/digital/io/src/ai_loader_cb.c +++ /dev/null @@ -1,636 +0,0 @@ -/* ai_loader_cb.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 "fsm.h" -#include "ai_cb.h" -#include "asserv.h" -#include "mimot.h" -#include "bot.h" -#include "main.h" -#include "food.h" -#include "loader.h" - -/* - * LOADER_IDLE =start=> - * => LOADER_WAIT_JACK_IN - */ -fsm_branch_t -ai__LOADER_IDLE__start (void) -{ - return ai_next (LOADER_IDLE, start); -} - -/* - * LOADER_WAIT_JACK_IN =jack_inserted_into_bot=> - * => LOADER_WAIT_JACK_OUT - */ -fsm_branch_t -ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void) -{ - return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot); -} - -/* - * LOADER_WAIT_JACK_IN =hola_start=> - * => LOADER_IDLE - */ -fsm_branch_t -ai__LOADER_WAIT_JACK_IN__hola_start (void) -{ - return ai_next (LOADER_WAIT_JACK_IN, hola_start); -} - -/* - * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=> - * => LOADER_INIT_ELEVATOR_ZERO - * find elevator zero - * close clamp - * find gate zero - */ -fsm_branch_t -ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) -{ - asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); - mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); - mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); - asserv_motor1_zero_position (-BOT_GATE_SPEED); - return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); -} - -/* - * LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=> - * => LOADER_INIT_CLAMP_CLOSE - */ -fsm_branch_t -ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void) -{ - return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); -} - -/* - * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=> - * => LOADER_INIT_CLAMP_ZERO - * move elevator up - * find clamp zero - */ -fsm_branch_t -ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, - BOT_ELEVATOR_ZERO_SPEED); - mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED); - mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED); - return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed); -} - -/* - * LOADER_INIT_CLAMP_ZERO =clamp_succeed=> - * => LOADER_INIT_GATE_ZERO - */ -fsm_branch_t -ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void) -{ - return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed); -} - -/* - * LOADER_INIT_GATE_ZERO =gate_succeed=> - * => LOADER_INIT_ELEVATOR_UP - */ -fsm_branch_t -ai__LOADER_INIT_GATE_ZERO__gate_succeed (void) -{ - return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed); -} - -/* - * LOADER_INIT_ELEVATOR_UP =elevator_succeed=> - * => LOADER_INIT_GATE_WAIT - */ -fsm_branch_t -ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void) -{ - return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed); -} - -/* - * LOADER_INIT_ELEVATOR_UP =elevator_failed=> - * => LOADER_IDLE - * initialisation failure - */ -fsm_branch_t -ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void) -{ - /* Move so that the operator notice it. */ - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3, - BOT_ELEVATOR_ZERO_SPEED); - return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed); -} - -/* - * LOADER_INIT_GATE_WAIT =state_timeout=> - * => LOADER_INIT_GATE_CLOSE - * close gate - */ -fsm_branch_t -ai__LOADER_INIT_GATE_WAIT__state_timeout (void) -{ - asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); - return ai_next (LOADER_INIT_GATE_WAIT, state_timeout); -} - -/* - * LOADER_INIT_GATE_CLOSE =gate_succeed=> - * => LOADER_UP - */ -fsm_branch_t -ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void) -{ - return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed); -} - -/* - * LOADER_UP =loader_down=> - * => LOADER_DOWNING - * move down - */ -fsm_branch_t -ai__LOADER_UP__loader_down (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_UP, loader_down); -} - -/* - * LOADER_DOWN =loader_up=> - * => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_DOWN__loader_up (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_DOWN, loader_up); -} - -/* - * LOADER_DOWN =loader_element=> - * => LOADER_LOAD_CLOSING - * clamp - */ -fsm_branch_t -ai__LOADER_DOWN__loader_element (void) -{ - mimot_motor0_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); - mimot_motor1_clamp (BOT_CLAMP_SPEED, BOT_CLAMP_PWM); - return ai_next (LOADER_DOWN, loader_element); -} - -/* - * LOADER_UPING =elevator_succeed=> - * => LOADER_UP - * post loader_uped event - */ -fsm_branch_t -ai__LOADER_UPING__elevator_succeed (void) -{ - main_post_event (AI_EVENT_loader_uped); - return ai_next (LOADER_UPING, elevator_succeed); -} - -/* - * LOADER_UPING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_UPING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_UPING, elevator_failed); -} - -/* - * LOADER_UPING =loader_down=> - * => LOADER_DOWNING - * move down - */ -fsm_branch_t -ai__LOADER_UPING__loader_down (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_UPING, loader_down); -} - -/* - * LOADER_DOWNING =elevator_succeed=> - * => LOADER_DOWN - * release elevator motor - * post loader_downed event - */ -fsm_branch_t -ai__LOADER_DOWNING__elevator_succeed (void) -{ - asserv_motor0_free (); - main_post_event (AI_EVENT_loader_downed); - return ai_next (LOADER_DOWNING, elevator_succeed); -} - -/* - * LOADER_DOWNING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_DOWNING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_DOWNING, elevator_failed); -} - -/* - * LOADER_DOWNING =loader_element=> - * => LOADER_ERROR - * emergency stop - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_DOWNING__loader_element (void) -{ - asserv_motor0_free (); - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_DOWNING, loader_element); -} - -/* - * LOADER_DOWNING =loader_up=> - * => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_DOWNING__loader_up (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_DOWNING, loader_up); -} - -/* - * LOADER_ERROR =loader_down=> - * => LOADER_ERROR_DOWNING - * move down - */ -fsm_branch_t -ai__LOADER_ERROR__loader_down (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_ERROR, loader_down); -} - -/* - * LOADER_ERROR =loader_up=> - * => LOADER_ERROR_UPING - * move up - */ -fsm_branch_t -ai__LOADER_ERROR__loader_up (void) -{ - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_ERROR, loader_up); -} - -/* - * LOADER_ERROR_DOWNING =elevator_succeed=> - * => LOADER_ERROR_DOWNING_OPEN - * release elevator motor - * open clamp - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING__elevator_succeed (void) -{ - asserv_motor0_free (); - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_ERROR_DOWNING, elevator_succeed); -} - -/* - * LOADER_ERROR_DOWNING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING, elevator_failed); -} - -/* - * LOADER_ERROR_DOWNING =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING, state_timeout); -} - -/* - * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=> - * => LOADER_DOWN - * post loader_downed event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void) -{ - main_post_event (AI_EVENT_loader_downed); - return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); -} - -/* - * LOADER_ERROR_DOWNING_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed); -} - -/* - * LOADER_ERROR_DOWNING_OPEN =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_DOWNING_OPEN__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_DOWNING_OPEN, state_timeout); -} - -/* - * LOADER_ERROR_UPING =elevator_succeed=> - * => LOADER_ERROR_UPING_OPEN - * open clamp - */ -fsm_branch_t -ai__LOADER_ERROR_UPING__elevator_succeed (void) -{ - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_ERROR_UPING, elevator_succeed); -} - -/* - * LOADER_ERROR_UPING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING, elevator_failed); -} - -/* - * LOADER_ERROR_UPING =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING, state_timeout); -} - -/* - * LOADER_ERROR_UPING_OPEN =clamp_succeed=> - * => LOADER_UP - * post loader_uped event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void) -{ - main_post_event (AI_EVENT_loader_uped); - return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed); -} - -/* - * LOADER_ERROR_UPING_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed); -} - -/* - * LOADER_ERROR_UPING_OPEN =state_timeout=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_ERROR_UPING_OPEN__state_timeout (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_ERROR_UPING_OPEN, state_timeout); -} - -/* - * LOADER_LOAD_CLOSING =clamp_succeed=> - * full => LOADER_LOAD_UPING - * move up - * count one element - * empty => LOADER_LOAD_EMPTY_OPEN - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_CLOSING__clamp_succeed (void) -{ - /* Measure load using clamp position. */ - uint16_t tickness = BOT_CLAMP_WIDTH_STEP - - mimot_get_motor0_position () - - mimot_get_motor1_position (); - if (tickness > BOT_CLAMP_EMPTY_STEP) - { - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, - BOT_ELEVATOR_SPEED); - position_t robot_position; - asserv_get_position (&robot_position); - food_taken (robot_position); - loader_elements++; - return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full); - } - else - { - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty); - } -} - -/* - * LOADER_LOAD_UPING =elevator_unload_position=> - * => LOADER_LOAD_UNLOADING - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_UPING__elevator_unload_position (void) -{ - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_LOAD_UPING, elevator_unload_position); -} - -/* - * LOADER_LOAD_UPING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored or loader_black event - * remove one element - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_UPING__elevator_failed (void) -{ - if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP) - { - if (loader_elements) - loader_elements--; - main_post_event (AI_EVENT_loader_black); - } - else - main_post_event (AI_EVENT_loader_errored); - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_LOAD_UPING, elevator_failed); -} - -/* - * LOADER_LOAD_UNLOADING =elevator_succeed=> - * => LOADER_LOAD_UNLOADING_OPEN - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING__elevator_succeed (void) -{ - return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed); -} - -/* - * LOADER_LOAD_UNLOADING =elevator_failed=> - * => LOADER_ERROR - * post loader_errored event - * open clamp - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING__elevator_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); - return ai_next (LOADER_LOAD_UNLOADING, elevator_failed); -} - -/* - * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=> - * down => LOADER_DOWNING - * move down - * up => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void) -{ - if (loader_want_up) - { - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, - BOT_ELEVATOR_SPEED); - return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up); - } - else - { - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, - BOT_ELEVATOR_SPEED); - return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down); - } -} - -/* - * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed); -} - -/* - * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=> - * down => LOADER_DOWN - * post loader_downed event - * up => LOADER_UPING - * move up - */ -fsm_branch_t -ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void) -{ - if (loader_want_up) - { - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, - BOT_ELEVATOR_SPEED); - return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up); - } - else - { - main_post_event (AI_EVENT_loader_downed); - return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); - } -} - -/* - * LOADER_LOAD_EMPTY_OPEN =clamp_failed=> - * => LOADER_ERROR - * post loader_errored event - */ -fsm_branch_t -ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void) -{ - main_post_event (AI_EVENT_loader_errored); - return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed); -} - - -- cgit v1.2.3