From b33059bcc5557a6565c9229cafd3b9a546a87b2d Mon Sep 17 00:00:00 2001 From: Julien Balleyguier Date: Mon, 7 May 2012 13:31:50 +0200 Subject: digital/io-hub/src/guybrush: Clamp fsm and all the needed files to simulate it --- digital/ai/tools/test_simu_control_guybrush.py | 30 +- digital/io-hub/src/guybrush/Makefile | 3 +- digital/io-hub/src/guybrush/bottom_clamp.c | 440 +++++++++++++++++++++++++ digital/io-hub/src/guybrush/main.c | 37 ++- digital/io-hub/tools/io_hub/io_hub.py | 9 +- 5 files changed, 513 insertions(+), 6 deletions(-) create mode 100644 digital/io-hub/src/guybrush/bottom_clamp.c (limited to 'digital') diff --git a/digital/ai/tools/test_simu_control_guybrush.py b/digital/ai/tools/test_simu_control_guybrush.py index 407648dc..146ad04a 100644 --- a/digital/ai/tools/test_simu_control_guybrush.py +++ b/digital/ai/tools/test_simu_control_guybrush.py @@ -77,6 +77,22 @@ class TestSimuControl (TestSimu): out_button ('Door open', 1 << defs.OUTPUT_DOOR_OPEN | 1 << defs.OUTPUT_DOOR_CLOSE) + self.tree_detected_button = Button (self.control_frame, + text = 'Tree Detected', padx = 0, pady = 0, + command = self.tree_detected) + self.tree_detected_button.pack() + self.stop_tree_approach_button = Button (self.control_frame, + text = 'Stop tree approach', padx = 0, pady = 0, + command = self.stop_tree_approach) + self.stop_tree_approach_button.pack() + self.empty_tree_button = Button (self.control_frame, + text = 'Empty Tree', padx = 0, pady = 0, + command = self.empty_tree) + self.empty_tree_button.pack() + self.robot_is_back_button = Button (self.control_frame, + text = 'Robot is back', padx = 0, pady = 0, + command = self.robot_is_back) + self.robot_is_back_button.pack() self.backward_var = IntVar () self.backward_button = Checkbutton (self.control_frame, text = 'Backward', variable = self.backward_var) @@ -106,7 +122,19 @@ class TestSimuControl (TestSimu): self.asserv.goto_angle (a) def lower_clamp_rotate_command (self): - self.mimot.speed_pos ('a0', self.LOWER_CLAMP_ROTATION_STROKE / 2) + self.mimot.speed_pos ('a0', self.LOWER_CLAMP_ROTATION_STROKE / 2) + + def tree_detected(self): + self.io.tree_detected() + + def stop_tree_approach(self): + self.io.stop_tree_approach() + + def empty_tree(self): + self.io.empty_tree() + + def robot_is_back(self): + self.io.robot_is_back() if __name__ == '__main__': run ('guybrush', TestSimuControl) diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile index 7e3db6fd..9099dc20 100644 --- a/digital/io-hub/src/guybrush/Makefile +++ b/digital/io-hub/src/guybrush/Makefile @@ -10,7 +10,8 @@ io_hub_SOURCES = main.c top.c \ contact.avr.c contact.host.c \ output.c output.host.c \ twi_master.c asserv.c mimot.c beacon.c \ - chrono.c timer.avr.c simu.host.c + chrono.c timer.avr.c simu.host.c \ + bottom_clamp.c # Modules needed for IO. MODULES = proto uart twi utils \ adc devices/usdist \ diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c new file mode 100644 index 00000000..55f913a5 --- /dev/null +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -0,0 +1,440 @@ +/* bottom_clamp.c */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 Julien Balleyguier + * + * 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 "output.h" + +#include "mimot.h" +#include "contact.h" +#include "bot.h" + + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.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 the 2 clamps */ + CLAMP_INIT_OPEN, + /* Finding 0 position */ + CLAMP_INIT_FIND_0, + /* Hide the clamp in order not to go over the robot. */ + CLAMP_INIT_HIDE_CLAMP, + /* 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 a coin */ + CLAMP_TAKE_COIN, + /* turning half way */ + CLAMP_TURN_HALF_WAY, + /* Droping the cd. */ + CLAMP_DROP_CD, + /* Clamp blocked. */ + CLAMP_BLOCKED, + BOTTOM_CLAMP_HIDE_POS, + UNFOLD_UPPER_SET, + BOTTOM_CLAMP_READY, + READY_TO_EMPTY_TREE, + CLOSE_ALL_CLAMPS, + REARRANGE_CD, + BOTTOM_CLAMP_HIDE_POS2, + RELEASE_ASSERV, + FOLD_UPPER_SET, + OPEN_UPPER_CLAMPS, + REARRANGE_CD_CLAMP, + BOTOM_CLAMP_BACK + ) + +FSM_EVENTS ( + /* coin detected in the clamp */ + coin_detected, + /* Lower clamp rotation motor success. */ + lower_clamp_rotation_success, + /* Lower clamp rotation motor failure. */ + lower_clamp_rotation_failure, + /*upper set is totally open*/ + upper_set_up, + /*upper set is totally close*/ + upper_set_down, + /* Tree detected, get ready to empty it. */ + tree_detected, + /* stop the tree-emptying cycle*/ + stop_tree_approach, + /* signal sent when the robot is ready to empty the tree. the approach has been finished*/ + clamps_ready, + /* the robot is ready to empty the tree, and the top fsm is asking him to do so*/ + empty_tree, + robot_is_back + ) + + +FSM_START_WITH (CLAMP_IDLE) + +/*------------------------------------- + TIMEOUT DEFINITION +---------------------------------------*/ +#define TIMEOUT_OPEN_CLAMPS 40 +#define TIMEOUT_CLOSE_CLAMPS 40 +#define TIMEOUT_DROP_CD 100 +#define TIMEOUT_FREE_ASSERV 100 +#define TIMEOUT_RECENTRAGE 100 + +/*------------------------------------- + ROTATION DEFINITION +---------------------------------------*/ +#define HIDE_POS 3 +#define BACK_TO_READY 16-HIDE_POS +#define HALF_TURN 8 +#define HIDE_POS_TREE 3 +#define HIDE_POS_TREE_2 1 +#define BACK_TO_READY_TREE 16-HIDE_POS_TREE +#define SPEED_ROTATION 0x20 +/*------------------------------------- + Clamp context +---------------------------------------*/ +struct clamp_t +{ + /** Current position. */ + int pos_current; + /* Clamp_1 is low, which means, it is ready to take a coin*/ + uint8_t clamp_1_down; + /** True if clamp 1 is open. */ + uint8_t clamp_1_open; + /** True if clamp 2 is open. */ + uint8_t clamp_2_open; +}; + + +static void +clamp_blocked (void) +{ +} + +/*Global context. */ +struct clamp_t clamp_global; +#define ctx clamp_global + +/*---------------------------------------------------------*/ +/* INIT part of the FSM */ +/*---------------------------------------------------------*/ + +/* Init Bottom clamp. */ + +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN) +{ + /* Opening the 2 clamps. */ + ctx.clamp_1_open = 1; + IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE); + ctx.clamp_2_open = 1; + IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE); + /* recentrage the middle clamp. */ + IO_SET (OUTPUT_UPPER_CLAMP_OUT); + IO_CLR (OUTPUT_UPPER_CLAMP_IN); + + return FSM_NEXT (CLAMP_START,init_actuators); + +} + +FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN, TIMEOUT_OPEN_CLAMPS, CLAMP_INIT_FIND_0) +{ + /*Findig the 0 position. */ + mimot_motor0_find_zero (0x05, 1, 0); + ctx.pos_current = 0; + return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN); +} + +FSM_TRANS (CLAMP_INIT_FIND_0, lower_clamp_rotation_success, CLAMP_INIT_HIDE_CLAMP) +{ + /* Hidding the clamp inside the robot. */ + int move_needed; + move_needed = HIDE_POS * 250; + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + return FSM_NEXT (CLAMP_INIT_FIND_0, lower_clamp_rotation_success); +} + +FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success, CLAMP_INIT_READY) +{ + /*Nothing to do,just waiting for the clamp to be hidden*/ + return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success); +} + + +FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_failure, CLAMP_BLOCKED) +{ + /*The clamp is blocked somehow*/ + clamp_blocked(); + return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_failure); +} + +FSM_TRANS (CLAMP_INIT_READY,init_start_round, CLAMP_GOING_IDLE) +{ + int move_needed; + move_needed = BACK_TO_READY * 250; + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + ctx.clamp_1_down = 1; + return FSM_NEXT (CLAMP_INIT_READY, init_start_round); +} + +FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_success, CLAMP_IDLE) +{ + /*Going back to the idle position, ready for showtime.*/ + return FSM_NEXT (CLAMP_GOING_IDLE, lower_clamp_rotation_success); +} + +FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_failure, CLAMP_BLOCKED) +{ + /* The clamp is blocked somehow. */ + clamp_blocked(); + return FSM_NEXT (CLAMP_GOING_IDLE, lower_clamp_rotation_failure); +} + + +/*---------------------------------------------------------*/ +/* parts of the FSM that Takes coin */ +/*---------------------------------------------------------*/ +FSM_TRANS (CLAMP_IDLE, coin_detected, CLAMP_TAKE_COIN) +{ + if (ctx.clamp_1_down) + { + IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); //close it + ctx.clamp_1_open = 0; + } + else // if the clamp 2 is ready + { + IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE); + ctx.clamp_2_open = 0; + } + return FSM_NEXT (CLAMP_IDLE, coin_detected); +} + + +FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY) +{ + int move_needed; + move_needed = HALF_TURN * 250; + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + return FSM_NEXT_TIMEOUT (CLAMP_TAKE_COIN); +} + +FSM_TRANS_TIMEOUT (CLAMP_TURN_HALF_WAY, TIMEOUT_DROP_CD, CLAMP_DROP_CD) +{ + /*If the clamp 1 is closed.*/ + if (!ctx.clamp_1_open) + { + /*Open it.*/ + IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE); + ctx.clamp_1_open = 1; + /*Clamp 1 is now up (clamp 2 is down).*/ + ctx.clamp_1_down = 0; + } + /*If the clamp 2 is closed. */ + else + { + IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE); + ctx.clamp_2_open = 1; + /*Clamp 1 is now down (clamp 2 is up). */ + ctx.clamp_1_down = 1; + } + return FSM_NEXT_TIMEOUT (CLAMP_TURN_HALF_WAY); +} + +FSM_TRANS (CLAMP_DROP_CD,lower_clamp_rotation_success,CLAMP_IDLE) +{ + /*Nothing to do, just waiting to be in IDLE state again*/ + return FSM_NEXT (CLAMP_DROP_CD,lower_clamp_rotation_success); +} + +FSM_TRANS (CLAMP_DROP_CD, lower_clamp_rotation_failure, CLAMP_BLOCKED) +{ + /*The clamp is blocked somehow. */ + clamp_blocked(); + return FSM_NEXT (CLAMP_DROP_CD, lower_clamp_rotation_failure); +} +/*---------------------------------------------------------*/ +/* Parts of the FSM that is Approaching the tree (totem) */ +/*---------------------------------------------------------*/ + +FSM_TRANS (CLAMP_IDLE, tree_detected, BOTTOM_CLAMP_HIDE_POS) +{ + /*Contrepression*/ + IO_SET (OUTPUT_UPPER_CLAMP_UP); + /*Hidding the clamp inside the robot*/ + int move_needed; + if (ctx.clamp_1_down) + { + move_needed = HIDE_POS_TREE * 250; + } + else + { + move_needed = (HALF_TURN + HIDE_POS_TREE) * 250; + ctx.clamp_1_down = 1; + } + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + return FSM_NEXT (CLAMP_IDLE, tree_detected); +} + + +FSM_TRANS (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success, UNFOLD_UPPER_SET) +{ + IO_CLR (OUTPUT_UPPER_CLAMP_UP); + IO_SET (OUTPUT_UPPER_CLAMP_DOWN); + return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success); +} + +FSM_TRANS (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure, CLAMP_BLOCKED) +{ + /*The clamp is blocked somehow*/ + clamp_blocked(); + return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure); +} + + +FSM_TRANS (UNFOLD_UPPER_SET, upper_set_down, BOTTOM_CLAMP_READY) +{ + /*putting the bottom clamp back to ready*/ + int move_needed; + move_needed = BACK_TO_READY_TREE * 250; + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + + /*opening the top clamp*/ + IO_SET (OUTPUT_UPPER_CLAMP_OPEN); + + return FSM_NEXT (UNFOLD_UPPER_SET, upper_set_down); + +} + +FSM_TRANS (BOTTOM_CLAMP_READY, lower_clamp_rotation_success, READY_TO_EMPTY_TREE) +{ + + fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); + return FSM_NEXT (BOTTOM_CLAMP_READY, lower_clamp_rotation_success); + +} + +FSM_TRANS (BOTTOM_CLAMP_READY, lower_clamp_rotation_failure, CLAMP_BLOCKED) +{ + /*The clamp is blocked somehow*/ + clamp_blocked(); + return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure); +} + +FSM_TRANS (READY_TO_EMPTY_TREE, empty_tree, CLOSE_ALL_CLAMPS) +{ + /*Closgin bottom clamp*/ + IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); + ctx.clamp_1_open=0; + // + /*(We need to close both clamp to have an easier turn around) */ + IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE); + ctx.clamp_2_open=0; + + + /*closing upper & middle clamps*/ + IO_CLR (OUTPUT_UPPER_CLAMP_OPEN); + + + return FSM_NEXT (READY_TO_EMPTY_TREE, empty_tree); + +} + +/*---------------------------------------------------------*/ +/* Parts of the FSM that is emptying the tree (totem) */ +/*---------------------------------------------------------*/ + +FSM_TRANS(CLOSE_ALL_CLAMPS, robot_is_back,REARRANGE_CD) +{ + IO_CLR (OUTPUT_UPPER_CLAMP_OUT); + IO_SET (OUTPUT_UPPER_CLAMP_IN); + return FSM_NEXT (CLOSE_ALL_CLAMPS, robot_is_back); + +} + +FSM_TRANS_TIMEOUT (REARRANGE_CD, TIMEOUT_RECENTRAGE, BOTTOM_CLAMP_HIDE_POS2) +{ + /*Hidding the clamp inside the robot*/ + int move_needed; + move_needed = HIDE_POS_TREE_2 * 250; + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + return FSM_NEXT_TIMEOUT (REARRANGE_CD); +} +FSM_TRANS (BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success, RELEASE_ASSERV) +{ + mimot_motor0_free(); + return FSM_NEXT (BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success); + +} + +FSM_TRANS_TIMEOUT (RELEASE_ASSERV, TIMEOUT_FREE_ASSERV, FOLD_UPPER_SET) +{ + IO_CLR (OUTPUT_UPPER_CLAMP_DOWN); + IO_SET (OUTPUT_UPPER_CLAMP_UP); + return FSM_NEXT_TIMEOUT (RELEASE_ASSERV); + +} +FSM_TRANS (FOLD_UPPER_SET, upper_set_up, OPEN_UPPER_CLAMPS) +{ + IO_SET (OUTPUT_UPPER_CLAMP_OPEN); + IO_CLR (OUTPUT_UPPER_CLAMP_IN); + IO_SET (OUTPUT_UPPER_CLAMP_OUT); + return FSM_NEXT (FOLD_UPPER_SET, upper_set_up); + +} + +FSM_TRANS_TIMEOUT (OPEN_UPPER_CLAMPS, TIMEOUT_OPEN_CLAMPS, CLAMP_TURN_HALF_WAY) +{ + IO_SET (OUTPUT_UPPER_CLAMP_OPEN); + /*we reopen clamp 2*/ + IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE); + ctx.clamp_2_open = 1; + int move_needed; + move_needed = (HALF_TURN-HIDE_POS_TREE_2) * 250; + ctx.pos_current += move_needed; + mimot_move_motor0_absolute (ctx.pos_current, SPEED_ROTATION); + return FSM_NEXT_TIMEOUT (OPEN_UPPER_CLAMPS); + +} + diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index 79940eac..f73ec254 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -158,11 +158,24 @@ main_event_to_fsm (void) FSM_HANDLE_E (AI, robot_move_success); else if (robot_move_status == failure) FSM_HANDLE_E (AI, robot_move_failure); + if (mimot_motor0_status == success) + FSM_HANDLE_E (AI, lower_clamp_rotation_success); + else if (mimot_motor0_status == failure) + FSM_HANDLE_E (AI, lower_clamp_rotation_failure); + if (!IO_GET (CONTACT_LOWER_CLAMP_SENSOR_1) + || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_2) + || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_3) + || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_4)) + FSM_HANDLE_E (AI, coin_detected); /* Jack. */ if (!contact_get_jack ()) FSM_HANDLE_E (AI, jack_inserted); else FSM_HANDLE_E (AI, jack_removed); + if (!IO_GET(CONTACT_UPPER_CLAMP_DOWN)) + FSM_HANDLE_E (AI,upper_set_down); + if (!IO_GET(CONTACT_UPPER_CLAMP_UP)) + FSM_HANDLE_E (AI,upper_set_up); /* Events from the event queue. */ if (fsm_queue_poll ()) { @@ -269,6 +282,26 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Enter FSM debug mode, then step once. */ main_fsm_debug_mode = MAIN_FSM_DEBUG_STEP; break; + case c ('t',0): + /* Simulate tree detection. */ + fsm_queue_post_event (FSM_EVENT (AI, tree_detected)); + break; + case c ('s',0): + /* Simulate stop tree approach. */ + fsm_queue_post_event (FSM_EVENT (AI, stop_tree_approach)); + break; + case c ('e',0): + /* Simulate the empty tree command. */ + fsm_queue_post_event (FSM_EVENT (AI, empty_tree)); + break; + case c ('r',0): + /* Simulate the robot_is_back command. */ + fsm_queue_post_event (FSM_EVENT (AI, robot_is_back)); + break; + case c ('c',0): + /* Simulate the coin detected command. */ + fsm_queue_post_event (FSM_EVENT (AI, coin_detected)); + break; case c ('m', 5): /* Go to position. * - 2w: x, y. @@ -282,9 +315,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('w', 0): /* Disable all motor control. */ - mimot_motor0_free (); - mimot_motor1_free (); - asserv_free_motor (); + mimot_reset(); break; case c ('o', 5): /* Set/clear/toggle outputs. diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index 6623a346..4aa388b0 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -92,4 +92,11 @@ class ProtoRobospierre (Proto): class ProtoGuybrush (Proto): - pass + def tree_detected(self): + self.proto.send('t') + def stop_tree_approach(self): + self.proto.send('s') + def empty_tree(self): + self.proto.send('e') + def robot_is_back(self): + self.proto.send('r') -- cgit v1.2.3