/* 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" #include "main.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, /*--------------------------IDLE---------------------------------------*/ /* Waiting external events. */ CLAMP_IDLE, /*---------------------------Taking coin sequence-----------------------*/ /*Taking a coin. */ CLAMP_TAKE_COIN, /*Turning half way. */ CLAMP_TURN_HALF_WAY, /* Droping the cd. */ CLAMP_DROP_CD, /*--------------------------Approaching Tree sequence------------------*/ /*Hide bottom clamp to unfold the upper set. */ CLAMP_BOTTOM_CLAMP_HIDE_POS, /*unfold the upper set. */ CLAMP_UNFOLD_UPPER_SET, /*Putting the bottom clamp back in position. */ CLAMP_BOTTOM_CLAMP_READY, /*All the clamps are now open and the robot is ready to empty the tree. */ CLAMP_READY_TO_EMPTY_TREE, /*Close all clamps to empty the tree. */ CLAMP_CLOSE_ALL_CLAMPS, /*Recentrage of the CD clamps. */ CLAMP_REARRANGE_CD, /*Hiding bottom clamp to fold back the upper set. */ CLAMP_BOTTOM_CLAMP_HIDE_POS2, /*Release the asserve not to block the clamp when going back up. */ CLAMP_RELEASE_ASSERV, /*fold back the upper set. */ CLAMP_FOLD_UPPER_SET, /*Open upper clamps to release objects. */ CLAMP_OPEN_UPPER_CLAMPS, /*-------------------------Stop tree approach extra states--------------*/ /*Letting the bottom clamp to finish it's move before starting the stop tree procedure. */ CLAMP_FINISH_BOTTOM_CLAMP_READY, /*Putting the bottom clamp back in position after a stop tree approach signal. */ CLAMP_BOTTOM_CLAMP_BACK, /*------------------------ Clamp blocked------------------------------. */ CLAMP_BLOCKED, CLAMP_OPEN_BOTTOM_CLAMPS, CLAMP_TURN_BACKWARD, CLAMP_WAIT, CLAMP_TURN_FORWARD, CLAMP_SHITTY_STATE ) FSM_EVENTS ( /* coin detected in the clamp */ coin_detected, /*signal sent to the top fsm when taking a coin*/ taking_coin, /*time to drop the cd. Sent by main*/ time_to_drop_coin, /* 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, clamp_blocked ) FSM_START_WITH (CLAMP_START) /*------------------------------------- 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 #define TIMEOUT_BLOCKED 100 /*------------------------------------- ROTATION DEFINITION ---------------------------------------*/ #define POS_DELAY 1250 #define HIDE_POS 3 #define BACK_TO_READY (16-HIDE_POS) #define HALF_TURN 8 #define QUARTER_TURN (16-4) #define HIDE_POS_TREE 3 #define HIDE_POS_TREE_2 2 #define BACK_TO_READY_TREE -HIDE_POS_TREE #define BACK_TO_READY_TREE_2 (HALF_TURN-HIDE_POS_TREE_2) #define SPEED_ROTATION 0x30 #define FAILURE_ROTATION 0x10 /*------------------------------------- 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 we are stopping the tree approach. */ uint8_t stop_tree_approach; }; /*Global context. */ struct clamp_t clamp_global; #define ctx clamp_global static void clamp_blocked (void) { } static void move_needed(int move,int speed) { ctx.pos_current += move; mimot_move_motor0_absolute (ctx.pos_current, speed); } /*---------------------------------------------------------*/ /* INIT part of the FSM */ /*---------------------------------------------------------*/ /* Init Bottom clamp. */ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN) { /* Opening the 2 clamps. */ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE); 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. */ move_needed(HIDE_POS * 250,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_READY,init_start_round, CLAMP_GOING_IDLE) { move_needed(BACK_TO_READY * 250,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.*/ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT (CLAMP_GOING_IDLE, lower_clamp_rotation_success); } /*---------------------------------------------------------*/ /* parts of the FSM that Takes coin */ /*---------------------------------------------------------*/ FSM_TRANS (CLAMP_IDLE, coin_detected, CLAMP_TAKE_COIN) { if (ctx.clamp_1_down) { /*Close it.*/ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); } /*If the clamp 2 is ready*/ else { IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE); } fsm_queue_post_event (FSM_EVENT (AI, taking_coin)); return FSM_NEXT (CLAMP_IDLE, coin_detected); } FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY) { main_set_drop_coin_pos(ctx.pos_current + (HALF_TURN * 250) - POS_DELAY); move_needed(HALF_TURN * 250,SPEED_ROTATION); return FSM_NEXT_TIMEOUT (CLAMP_TAKE_COIN); } FSM_TRANS (CLAMP_TURN_HALF_WAY, time_to_drop_coin, CLAMP_DROP_CD) { /*If the clamp 1 has the CD.*/ if (ctx.clamp_1_down) { /*Open it.*/ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE); /*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); /*Clamp 1 is now down (clamp 2 is up). */ ctx.clamp_1_down = 1; } return FSM_NEXT (CLAMP_TURN_HALF_WAY,time_to_drop_coin); } FSM_TRANS (CLAMP_TURN_HALF_WAY, lower_clamp_rotation_failure, CLAMP_BLOCKED) { return FSM_NEXT (CLAMP_TURN_HALF_WAY,lower_clamp_rotation_failure); } FSM_TRANS (CLAMP_DROP_CD,lower_clamp_rotation_success,CLAMP_IDLE) { fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); 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, CLAMP_BOTTOM_CLAMP_HIDE_POS) { /*Contrepression*/ IO_SET (OUTPUT_UPPER_CLAMP_UP); /*Hidding the clamp inside the robot*/ if (ctx.clamp_1_down) { move_needed(HIDE_POS_TREE * 250,SPEED_ROTATION); } else { move_needed((HALF_TURN + HIDE_POS_TREE) * 250,SPEED_ROTATION); ctx.clamp_1_down = 1; } return FSM_NEXT (CLAMP_IDLE, tree_detected); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success, CLAMP_UNFOLD_UPPER_SET) { IO_CLR (OUTPUT_UPPER_CLAMP_UP); IO_SET (OUTPUT_UPPER_CLAMP_DOWN); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure, CLAMP_BLOCKED) { /*The clamp is blocked somehow.*/ clamp_blocked(); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure); } FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, upper_set_down, CLAMP_BOTTOM_CLAMP_READY) { /*Putting the bottom clamp back to ready.*/ move_needed(BACK_TO_READY_TREE * 250,SPEED_ROTATION); /*Opening the top clamp.*/ IO_SET (OUTPUT_UPPER_CLAMP_OPEN); return FSM_NEXT (CLAMP_UNFOLD_UPPER_SET, upper_set_down); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_success, CLAMP_READY_TO_EMPTY_TREE) { fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_success); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure, CLAMP_BLOCKED) { /*The clamp is blocked somehow.*/ clamp_blocked(); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure); } FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, empty_tree, CLAMP_CLOSE_ALL_CLAMPS) { /*Closgin bottom clamp. */ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); /*We need to close both clamp to have an easier turn around. */ IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE); /*Closing upper & middle clamps.*/ IO_CLR (OUTPUT_UPPER_CLAMP_OPEN); fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT (CLAMP_READY_TO_EMPTY_TREE, empty_tree); } /*---------------------------------------------------------*/ /* Parts of the FSM that is emptying the tree (totem) */ /*---------------------------------------------------------*/ FSM_TRANS(CLAMP_CLOSE_ALL_CLAMPS, robot_is_back,CLAMP_REARRANGE_CD) { IO_CLR (OUTPUT_UPPER_CLAMP_OUT); IO_SET (OUTPUT_UPPER_CLAMP_IN); return FSM_NEXT (CLAMP_CLOSE_ALL_CLAMPS, robot_is_back); } FSM_TRANS_TIMEOUT (CLAMP_REARRANGE_CD, TIMEOUT_RECENTRAGE, CLAMP_BOTTOM_CLAMP_HIDE_POS2) { /*Hidding the clamp inside the robot.*/ move_needed(HIDE_POS_TREE_2 * 250,SPEED_ROTATION); return FSM_NEXT_TIMEOUT (CLAMP_REARRANGE_CD); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success, CLAMP_RELEASE_ASSERV) { mimot_motor0_free(); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_failure, CLAMP_BLOCKED) { clamp_blocked(); return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_failure); } FSM_TRANS_TIMEOUT (CLAMP_RELEASE_ASSERV, TIMEOUT_FREE_ASSERV, CLAMP_FOLD_UPPER_SET) { IO_CLR (OUTPUT_UPPER_CLAMP_DOWN); IO_SET (OUTPUT_UPPER_CLAMP_UP); return FSM_NEXT_TIMEOUT (CLAMP_RELEASE_ASSERV); } FSM_TRANS (CLAMP_FOLD_UPPER_SET, upper_set_up, CLAMP_OPEN_UPPER_CLAMPS) { IO_SET (OUTPUT_UPPER_CLAMP_OPEN); IO_CLR (OUTPUT_UPPER_CLAMP_IN); IO_SET (OUTPUT_UPPER_CLAMP_OUT); return FSM_NEXT (CLAMP_FOLD_UPPER_SET, upper_set_up); } FSM_TRANS_TIMEOUT (CLAMP_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); if (ctx.stop_tree_approach) { main_set_drop_coin_pos(ctx.pos_current + ((HALF_TURN - HIDE_POS_TREE) * 250) - POS_DELAY); move_needed((HALF_TURN - HIDE_POS_TREE) * 250,SPEED_ROTATION); } else { main_set_drop_coin_pos(ctx.pos_current + (BACK_TO_READY_TREE_2 * 250) - POS_DELAY); move_needed((BACK_TO_READY_TREE_2) * 250,SPEED_ROTATION); } ctx.stop_tree_approach = 0; fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS); } /*---------------------------------------------------------------------------------*/ /*Parts of the FSM that goes back to idle after receiving the stop approach signal */ /*---------------------------------------------------------------------------------*/ FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS,stop_tree_approach,CLAMP_BOTTOM_CLAMP_BACK) { /*Putting the clamp back to take coin*/ if (ctx.clamp_1_down) { move_needed(BACK_TO_READY_TREE * 250,SPEED_ROTATION); } else { move_needed((16-BACK_TO_READY_TREE) * 250,SPEED_ROTATION); } return FSM_NEXT(CLAMP_BOTTOM_CLAMP_HIDE_POS,stop_tree_approach); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_success,CLAMP_IDLE) { fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT(CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_success); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_failure,CLAMP_BLOCKED) { clamp_blocked(); return FSM_NEXT(CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_failure); } FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, stop_tree_approach,CLAMP_RELEASE_ASSERV) { ctx.stop_tree_approach = 1; return FSM_NEXT (CLAMP_UNFOLD_UPPER_SET, stop_tree_approach); } FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, stop_tree_approach,CLAMP_FINISH_BOTTOM_CLAMP_READY) { return FSM_NEXT (CLAMP_BOTTOM_CLAMP_READY, stop_tree_approach); } FSM_TRANS (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_success,CLAMP_REARRANGE_CD) { return FSM_NEXT (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_success); } FSM_TRANS (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure,CLAMP_BLOCKED) { clamp_blocked(); return FSM_NEXT (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure); } FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach, CLAMP_REARRANGE_CD) { return FSM_NEXT (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach); } /*---------------------------------------------------------------------------------*/ /* Parts of the FSM that takes care of the bottom clamp when it's blocked */ /*---------------------------------------------------------------------------------*/ FSM_TRANS_TIMEOUT (CLAMP_BLOCKED,TIMEOUT_OPEN_CLAMPS,CLAMP_OPEN_BOTTOM_CLAMPS) { fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); /* Opening the 2 clamps. */ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE); IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE); return FSM_NEXT_TIMEOUT (CLAMP_BLOCKED); } FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,robot_is_back, CLAMP_WAIT) { mimot_move_motor0_absolute (mimot_get_motor0_position() - 16 * 250, FAILURE_ROTATION); return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, robot_is_back); } FSM_TRANS (CLAMP_WAIT,lower_clamp_rotation_success, CLAMP_TURN_BACKWARD) { return FSM_NEXT (CLAMP_WAIT, lower_clamp_rotation_success); } FSM_TRANS_TIMEOUT (CLAMP_TURN_BACKWARD,TIMEOUT_BLOCKED, CLAMP_TURN_FORWARD) { move_needed(0,FAILURE_ROTATION); if (ctx.clamp_1_down) { /*Clamp 1 is now up (clamp 2 is down).*/ ctx.clamp_1_down = 0; } else { ctx.clamp_1_down = 1; } return FSM_NEXT_TIMEOUT (CLAMP_TURN_BACKWARD); } FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_success, CLAMP_IDLE) { return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success); } FSM_TRANS (CLAMP_TURN_BACKWARD,lower_clamp_rotation_failure, CLAMP_SHITTY_STATE) { return FSM_NEXT (CLAMP_TURN_BACKWARD, lower_clamp_rotation_failure); } FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_failure, CLAMP_SHITTY_STATE) { return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_failure); }