From 1901265a3ea59267f92fe2e923184fce3d689f72 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Sat, 9 May 2009 00:10:02 +0200 Subject: * digital/io/src: - First version of init FSM --- digital/io/src/Makefile | 2 +- digital/io/src/asserv.c | 4 +- digital/io/src/asserv.h | 2 +- digital/io/src/fsm.h | 1 + digital/io/src/init.c | 25 +++++++++ digital/io/src/init.fsm | 73 ++++++++++++++++++++++++ digital/io/src/init.h | 31 ++++++++++ digital/io/src/init_cb.c | 143 +++++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/main.c | 55 ++++++++++++++++-- digital/io/src/main.h | 6 ++ 10 files changed, 332 insertions(+), 10 deletions(-) create mode 100644 digital/io/src/init.c create mode 100644 digital/io/src/init.fsm create mode 100644 digital/io/src/init.h create mode 100644 digital/io/src/init_cb.c (limited to 'digital/io/src') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 48511605..df2d6502 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -16,6 +16,6 @@ AVR_MCU = atmega128 OPTIMIZE = -O2 # FSMs. -FSM_SOURCES := top move filterbridge elevator cylinder +FSM_SOURCES := top move filterbridge elevator cylinder init # Include FSM makefile. include $(BASE)/make/Makefile.fsm diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index c9594d3d..bc973ec4 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -453,10 +453,10 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a) /* Go to the wall (moving backward). */ void -asserv_go_to_the_wall (void) +asserv_go_to_the_wall (uint8_t backward) { /* Put direction as parameters */ - asserv_twi_buffer_param[0] = 1; + asserv_twi_buffer_param[0] = backward; /* Send the go the wall command to the asserv board */ asserv_twi_send_command ('f', 1); } diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 52cd0440..17b396da 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -227,7 +227,7 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a); * Move class command. */ void -asserv_go_to_the_wall (void); +asserv_go_to_the_wall (uint8_t backward); /** * Move forward to approach a distributor. diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index cf5ce15e..e08605fe 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -112,5 +112,6 @@ fsm_handle_event (fsm_t *fsm, u8 event); #include "filterbridge_fsm.h" #include "elevator_fsm.h" #include "cylinder_fsm.h" +#include "init_fsm.h" #endif /* fsm_h */ diff --git a/digital/io/src/init.c b/digital/io/src/init.c new file mode 100644 index 00000000..7b0e584c --- /dev/null +++ b/digital/io/src/init.c @@ -0,0 +1,25 @@ +/* init.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2009 Nicolas Haller + * + * 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 "init.h" diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm new file mode 100644 index 00000000..db69e15c --- /dev/null +++ b/digital/io/src/init.fsm @@ -0,0 +1,73 @@ +# init FSM +# This FSM set the position of the bot with some move on wall +# This FSM is required because the meca team is "relou" +init + init FSM with setup move + +States: + IDLE + waiting for the beginning of the top FSM + WAIT_JACK_IN + waiting for the jack to be inserted into the bot + WAIT_2_SEC + waiting for operator's hand not on jack anymore + GOTO_THE_WALL + go to the wall for the first time + GO_BACKWARD + go backward for INIT_DIST millimeters + TURN_90_DEGREES_CCW + turn bot for 90 degrees counterclockwise + GOTO_THE_WALL_AGAIN + go to the wall for the second time + GO_BACKWARD_AGAIN + go backward for INIT_DIST millimeters again + TURN_180_DEGREES_CCW + turn bot for 180 degrees counterclockwise + SET_POSITION + tell the truth to asserv + +Events: + start + initialize the FSM + jack_inserted_into_bot + the jack is inserted into the bot + init_tempo_ended + the tempo is finished + move_done + the move is finished + +IDLE: + start -> WAIT_JACK_IN + wait for the jack to be inserted into the bot + +WAIT_JACK_IN: + jack_inserted_into_bot -> WAIT_2_SEC + wait for the operator hand disappears + +WAIT_2_SEC: + init_tempo_ended -> GOTO_THE_WALL + go to the first wall + +GOTO_THE_WALL: + move_done -> GO_BACKWARD + go backward for INIT_DIST millimeters + +GO_BACKWARD: + move_done -> TURN_90_DEGREES_CCW + turn bot for 90 degrees counterclockwise + +TURN_90_DEGREES_CCW: + move_done -> GOTO_THE_WALL_AGAIN + go to the wall for the second time + +GOTO_THE_WALL_AGAIN: + move_done -> GO_BACKWARD_AGAIN + go backward for INIT_DIST millimeters again + +GO_BACKWARD_AGAIN: + move_done -> TURN_180_DEGREES_CCW + turn bot for 180 degrees counterclockwise + +TURN_180_DEGREES_CCW: + move_done -> SET_POSITION + set real position to asserv diff --git a/digital/io/src/init.h b/digital/io/src/init.h new file mode 100644 index 00000000..6ce25300 --- /dev/null +++ b/digital/io/src/init.h @@ -0,0 +1,31 @@ +#ifndef init_h +#define init_h +/* init.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2009 Nicolas Haller + * + * 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. + * + * }}} */ + +/* INIT_DIST is the distance the bot move backward during init FSM*/ +#define INIT_DIST 150 + +#endif /* init_h */ diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c new file mode 100644 index 00000000..f05a5fef --- /dev/null +++ b/digital/io/src/init_cb.c @@ -0,0 +1,143 @@ +/* init_cb.c - init FSM callbacks. */ +/* {{{ + * + * Copyright (C) 2009 Nicolas Haller + * + * 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 "init_cb.h" +#include "asserv.h" +#include "init.h" +#include "playground.h" +#include "main.h" + +/* + * GOTO_THE_WALL_AGAIN =move_done=> + * => GO_BACKWARD_AGAIN + * go backward for INIT_DIST millimeters again + */ +fsm_branch_t +init__GOTO_THE_WALL_AGAIN__move_done (void) +{ + asserv_move_linearly(-INIT_DIST); + return init_next (GOTO_THE_WALL_AGAIN, move_done); +} + +/* + * TURN_180_DEGREES_CCW =move_done=> + * => SET_POSITION + * set real position to asserv + */ +fsm_branch_t +init__TURN_180_DEGREES_CCW__move_done (void) +{ + /* FIXME Value from spa^W marcel */ + asserv_set_position(300, PG_HEIGHT - 305, 0); + return init_next (TURN_180_DEGREES_CCW, move_done); +} + +/* + * IDLE =start=> + * => WAIT_JACK_IN + * wait for the jack to be inserted into the bot + */ +fsm_branch_t +init__IDLE__start (void) +{ + return init_next (IDLE, start); +} + +/* + * TURN_90_DEGREES_CCW =move_done=> + * => GOTO_THE_WALL_AGAIN + * go to the wall for the second time + */ +fsm_branch_t +init__TURN_90_DEGREES_CCW__move_done (void) +{ + asserv_go_to_the_wall(0); + return init_next (TURN_90_DEGREES_CCW, move_done); +} + +/* + * GO_BACKWARD =move_done=> + * => TURN_90_DEGREES_CCW + * turn bot for 90 degrees counterclockwise + */ +fsm_branch_t +init__GO_BACKWARD__move_done (void) +{ + asserv_move_angularly(90); + return init_next (GO_BACKWARD, move_done); +} + +/* + * GOTO_THE_WALL =move_done=> + * => GO_BACKWARD + * go backward for INIT_DIST millimeters + */ +fsm_branch_t +init__GOTO_THE_WALL__move_done (void) +{ + asserv_move_linearly(-INIT_DIST); + return init_next (GOTO_THE_WALL, move_done); +} + +/* + * WAIT_JACK_IN =jack_inserted_into_bot=> + * => WAIT_2_SEC + * wait for the operator hand disappears + */ +fsm_branch_t +init__WAIT_JACK_IN__jack_inserted_into_bot (void) +{ + /* launch the timer (2 sec)*/ + main_init_wait_cycle = 450; + return init_next (WAIT_JACK_IN, jack_inserted_into_bot); +} + +/* + * WAIT_2_SEC =init_tempo_ended=> + * => GOTO_THE_WALL + * go to the first wall + */ +fsm_branch_t +init__WAIT_2_SEC__init_tempo_ended (void) +{ + /* tell asserv to go FORWARD to the wall */ + asserv_go_to_the_wall(0); + return init_next (WAIT_2_SEC, init_tempo_ended); +} + +/* + * GO_BACKWARD_AGAIN =move_done=> + * => TURN_180_DEGREES_CCW + * turn bot for 180 degrees counterclockwise + */ +fsm_branch_t +init__GO_BACKWARD_AGAIN__move_done (void) +{ + asserv_move_angularly(180); + return init_next (GO_BACKWARD_AGAIN, move_done); +} + + diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 13b02353..c3f85770 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -88,6 +88,11 @@ uint8_t main_always_stop_for_obstacle = 1; */ uint16_t main_move_wait_cycle; +/** + * The same for init FSM + */ +uint16_t main_init_wait_cycle; + /** * Sharps stats counters. */ @@ -137,10 +142,18 @@ main_init (void) /* Path module */ path_init (PG_BORDER_DISTANCE, PG_BORDER_DISTANCE, PG_WIDTH - PG_BORDER_DISTANCE, PG_HEIGHT - PG_BORDER_DISTANCE); - /* Start the top FSM */ - //top_start (); + /* Init all FSM (except move FSM) */ fsm_init(&top_fsm); + fsm_init(&init_fsm); + fsm_init(&cylinder_fsm); + fsm_init(&elevator_fsm); + fsm_init(&filterbridge_fsm); + /* Start all FSM (except move FSM) */ fsm_handle_event (&top_fsm, TOP_EVENT_start); + fsm_handle_event (&init_fsm, INIT_EVENT_start); + /* fsm_handle_event (&top_fsm, TOP_EVENT_start); + fsm_handle_event (&top_fsm, TOP_EVENT_start); + fsm_handle_event (&top_fsm, TOP_EVENT_start); */ /* Sharp module */ sharp_init (); /* PWM module */ @@ -249,6 +262,8 @@ main_loop (void) /* Pass it to all the FSM that need it */ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_succeed); + FSM_HANDLE_EVENT (&init_fsm, + INIT_EVENT_move_done); } else if (move_status == failure) { @@ -257,9 +272,26 @@ main_loop (void) MOVE_EVENT_bot_move_failed); } /* Jack */ + if(switch_get_jack()) + { + FSM_HANDLE_EVENT (&top_fsm, + TOP_EVENT_jack_removed_from_bot); + } + else + { + FSM_HANDLE_EVENT (&top_fsm, + TOP_EVENT_jack_inserted_into_bot); + FSM_HANDLE_EVENT (&init_fsm, + INIT_EVENT_jack_inserted_into_bot); + } + /* FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? TOP_EVENT_jack_removed_from_bot : TOP_EVENT_jack_inserted_into_bot); + FSM_HANDLE_EVENT (&init_fsm, switch_get_jack () ? + INIT_EVENT_jack_removed_from_bot : + INIT_EVENT_jack_inserted_into_bot); + */ /* Settings acknowledge */ /* if (main_top_generate_settings_ack_event) @@ -348,6 +380,11 @@ main_loop (void) /* Reset stats counter */ main_stats_asserv_cpt_ = main_stats_asserv_; } + /* Init FSM timer */ + if(main_init_wait_cycle) + --main_init_wait_cycle; + if(!main_init_wait_cycle) + FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_init_tempo_ended); } } @@ -507,10 +544,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Stop motor */ asserv_stop_motor (); break; - case 'f': - /* Go to the wall */ - asserv_go_to_the_wall (); - break; case 'F': /* Go to the distributor */ asserv_go_to_distributor (); @@ -518,6 +551,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) } } break; + case c ('a', 2): + { + switch (args[0]) + { + case 'f': + /* Go to the wall */ + asserv_go_to_the_wall (args[1]); + break; + } + } case c ('a', 3): { switch (args[0]) diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 1b09a9f8..06f2a669 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -56,6 +56,12 @@ extern uint16_t main_move_wait_cycle; */ extern uint16_t main_getsamples_wait_cycle; +/** + * Post an event for the init loop to wait a certain amount of time before + * make any move + */ +extern uint16_t main_init_wait_cycle; + /** * Flag for homologation, to disable the path finding and always stop in front * of an obstacle and wait. -- cgit v1.2.3