/* top.c */ /* robospierre - Eurobot 2011 AI. {{{ * * Copyright (C) 2011 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 "playground_2011.h" #include "asserv.h" #define FSM_NAME AI #include "fsm.h" #include "logistic.h" #include "move.h" /* * Here is the top FSM. This FSM is suppose to give life to the robot with an * impression of intelligence... Well... */ FSM_INIT FSM_STATES ( /* Initial state. */ TOP_START, /* Going out of start area. */ TOP_GOING_OUT1, /* Problem going out, wait before retry. */ TOP_GOING_OUT1_BLOCK_WAIT, /* Going out, first pawn emplacement. */ TOP_GOING_OUT2, /* Problem going out, wait before retry. */ TOP_GOING_OUT2_BLOCK_WAIT, TOP_GOING_TO_DROP, TOP_GOING_TO_ELEMENT, /* Waiting clamp has finished its work. */ TOP_WAITING_CLAMP, /* Unblocking: waiting a little bit. */ TOP_UNBLOCKING_SHAKE_WAIT, /* Unblocking: shaking. */ TOP_UNBLOCKING_SHAKE, /* Waiting construction is ready to drop. */ TOP_WAITING_READY, /* Dropping, opening the doors. */ TOP_DROP_DROPPING, /* Dropping, clearing so that doors can be closed. */ TOP_DROP_CLEARING) FSM_START_WITH (TOP_START) /** Top context. */ struct top_t { /** Target element. */ uint8_t target_element_id; /** Chaos counter. */ uint8_t chaos; /** Broken clamp. */ uint8_t broken; }; /** Global context. */ struct top_t top_global; #define ctx top_global FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1) { element_init (); asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200), 0); return FSM_NEXT (TOP_START, init_start_round); } FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2) { asserv_goto (PG_X (1500 - 2 * 350), PG_Y (PG_LENGTH - 350), 0); return FSM_NEXT (TOP_GOING_OUT1, robot_move_success); } FSM_TRANS (TOP_GOING_OUT1, robot_move_failure, TOP_GOING_OUT1_BLOCK_WAIT) { return FSM_NEXT (TOP_GOING_OUT1, robot_move_failure); } FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1) { asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200 - (++ctx.chaos % 4) * 10), 0); return FSM_NEXT_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT); } static uint8_t top_go_element (void) { position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); element_t e = element_get (ctx.target_element_id); if (!ctx.broken) { if (e.attr & ELEMENT_GREEN) logistic_global.prepare = 0; else logistic_global.prepare = 1; } vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; move_start_noangle (element_pos, backward, 0); return 1; } static uint8_t top_go_drop (void) { position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_unload_best (robot_pos); position_t drop_pos; drop_pos.v = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ if (drop_pos.v.y > PG_LENGTH / 2) { drop_pos.v.y -= 350 / 2; drop_pos.a = PG_A_DEG (-90); } else { drop_pos.v.y += 350 / 2; drop_pos.a = PG_A_DEG (90); } if (logistic_global.collect_direction == DIRECTION_BACKWARD) drop_pos.a += POSITION_A_DEG (180); /* Go! */ move_start (drop_pos, backward); return 0; } static uint8_t top_decision (void) { /* If we can make a tower. */ if (logistic_global.construct_possible == 1 || (ctx.broken && logistic_global.construct_possible == 2)) return top_go_drop (); if (logistic_global.need_prepare) { clamp_prepare (ctx.broken ? 3 : 2); return top_go_drop (); } else return top_go_element (); } static void top_taken_pawn (void) { position_t robot_pos; asserv_get_position (&robot_pos); element_taken (element_nearest_element_id (robot_pos), ELEMENT_PAWN); } FSM_TRANS (TOP_GOING_OUT2, robot_move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop); case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element); } } FSM_TRANS (TOP_GOING_OUT2, robot_move_failure, TOP_GOING_OUT2_BLOCK_WAIT) { return FSM_NEXT (TOP_GOING_OUT2, robot_move_failure); } FSM_TRANS_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, 250, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { if (clamp_working ()) return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, clamp_working); switch (top_decision ()) { default: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, drop); case 1: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, element); } } FSM_TRANS (TOP_GOING_TO_DROP, move_success, ready, TOP_DROP_DROPPING, wait_clamp, TOP_WAITING_READY) { if (logistic_global.ready) { clamp_drop (logistic_global.collect_direction); return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready); } else { clamp_prepare (ctx.broken ? 3 : 1); return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } } FSM_TRANS (TOP_GOING_TO_DROP, move_failure, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { element_failure (ctx.target_element_id); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, drop); case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, element); } } FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP) { top_taken_pawn (); move_stop (); return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working); } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { element_taken (ctx.target_element_id, ELEMENT_PAWN); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element); } } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { element_failure (ctx.target_element_id); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop); case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, element); } } FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) { top_taken_pawn (); move_stop (); return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { switch (top_decision ()) { default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop); case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element); } } FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) { return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); } FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250, try_again, TOP_UNBLOCKING_SHAKE, tryout, TOP_WAITING_CLAMP) { if (ctx.chaos < 4) { int16_t dist = logistic_global.collect_direction == DIRECTION_FORWARD ? 100 : -100; asserv_move_linearly (++ctx.chaos % 2 ? -dist : dist); return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, try_again); } else { ctx.broken = 1; clamp_prepare (3); return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, tryout); } } FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_success, TOP_WAITING_CLAMP) { clamp_prepare (0); return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_success); } FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure, TOP_UNBLOCKING_SHAKE_WAIT) { return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure); } FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) { clamp_drop (logistic_global.collect_direction); return FSM_NEXT (TOP_WAITING_READY, clamp_done); } FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) { return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); } FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { element_down (ctx.target_element_id, ELEMENT_TOWER); asserv_move_linearly (logistic_global.collect_direction == DIRECTION_FORWARD ? 150 : -150); return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); } FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, element); } } FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, element); } }