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_hola_cb.c | 165 -------------------------------------------- 1 file changed, 165 deletions(-) delete mode 100644 digital/io/src/ai_hola_cb.c (limited to 'digital/io/src/ai_hola_cb.c') diff --git a/digital/io/src/ai_hola_cb.c b/digital/io/src/ai_hola_cb.c deleted file mode 100644 index 6b76b058..00000000 --- a/digital/io/src/ai_hola_cb.c +++ /dev/null @@ -1,165 +0,0 @@ -/* ai_hola_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 "playground.h" - -#include "modules/math/fixed/fixed.h" - -uint32_t hola_angle; -int16_t hola_timeout; - -/* - * HOLA_IDLE =start=> - * => HOLA_WAIT_HOLA - */ -fsm_branch_t -ai__HOLA_IDLE__start (void) -{ - return ai_next (HOLA_IDLE, start); -} - -/* - * HOLA_WAIT_HOLA =hola_start=> - * => HOLA_WAIT_JACK_IN - */ -fsm_branch_t -ai__HOLA_WAIT_HOLA__hola_start (void) -{ - return ai_next (HOLA_WAIT_HOLA, hola_start); -} - -/* - * HOLA_WAIT_JACK_IN =jack_inserted_into_bot=> - * => HOLA_WAIT_JACK_OUT - */ -fsm_branch_t -ai__HOLA_WAIT_JACK_IN__jack_inserted_into_bot (void) -{ - return ai_next (HOLA_WAIT_JACK_IN, jack_inserted_into_bot); -} - -/* - * HOLA_WAIT_JACK_OUT =jack_removed_from_bot=> - * => HOLA_ROUNDS - */ -fsm_branch_t -ai__HOLA_WAIT_JACK_OUT__jack_removed_from_bot (void) -{ - return ai_next (HOLA_WAIT_JACK_OUT, jack_removed_from_bot); -} - -/* - * HOLA_ROUNDS =asserv_last_cmd_ack=> - * no_timeout => HOLA_ROUNDS - * move clamp rounds - * timeout => HOLA_UP - * move up - * rotate - */ -fsm_branch_t -ai__HOLA_ROUNDS__asserv_last_cmd_ack (void) -{ - if (++hola_timeout < 150) - { - int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000; - int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000; - int32_t dx = BOT_CLAMP_OPEN_STEP - + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos); - int32_t dy = BOT_ELEVATOR_ZERO_SPEED - + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin); - mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED); - mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED); - asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED); - hola_angle += 0x1000000 / (225 / 12); - return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout); - } - else - { - asserv_set_speed (0x10, 0x1c, 0x10, 0x1c); - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3); - asserv_move_angularly (POSITION_A_DEG (174)); - return ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, timeout); - } -} - -/* - * HOLA_UP =bot_move_succeed=> - * => HOLA_CENTER - * rotate to center - */ -fsm_branch_t -ai__HOLA_UP__bot_move_succeed (void) -{ - asserv_set_speed (0x10, 0x20, 0x10, 0x20); - asserv_move_angularly (POSITION_A_DEG (-90)); - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED); - return ai_next (HOLA_UP, bot_move_succeed); -} - -/* - * HOLA_CENTER =bot_move_succeed=> - * => HOLA_CLAP_CLOSE - * close clamp - */ -fsm_branch_t -ai__HOLA_CENTER__bot_move_succeed (void) -{ - mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); - mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); - return ai_next (HOLA_CENTER, bot_move_succeed); -} - -/* - * HOLA_CLAP_CLOSE =clamp_succeed=> - * => HOLA_CLAP_OPEN - * open clamp - */ -fsm_branch_t -ai__HOLA_CLAP_CLOSE__clamp_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 (HOLA_CLAP_CLOSE, clamp_succeed); -} - -/* - * HOLA_CLAP_OPEN =clamp_succeed=> - * => HOLA_CLAP_CLOSE - * close clamp - */ -fsm_branch_t -ai__HOLA_CLAP_OPEN__clamp_succeed (void) -{ - mimot_motor0_clamp (BOT_CLAMP_SPEED, 0); - mimot_motor1_clamp (BOT_CLAMP_SPEED, 0); - return ai_next (HOLA_CLAP_OPEN, clamp_succeed); -} - - -- cgit v1.2.3