/* taz.c - Automate pour Taz, robot 2005. */ /* asserv - Position & speed motor control on a ATmega128. {{{ * * Copyright (C) 2005 Nicolas Schodet * * 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. * * Contact : * Web: http://perso.efrei.fr/~schodet/ * Email: * }}} */ /** FSM state. */ uint8_t taz_state; uint8_t taz_substate; /** Positions. */ #define taz_scale 0.05026548245743669181 static const uint16_t taz_rear_16 = taz_scale * 270 / 2; static const uint32_t taz_rear_32 = taz_rear_16 * 256; static const uint16_t taz_front_16 = taz_scale * 270 / 2; static const uint32_t taz_front_32 = taz_front_16 * 256; static const uint16_t taz_side_16 = taz_scale * 340 / 2; static const uint32_t taz_side_32 = taz_side_16 * 256; static const uint16_t taz_start_y_16 = taz_scale * 450 / 2; static const uint16_t taz_before_bridge_16 = taz_scale * 1050; static const uint16_t taz_after_bridge_16 = taz_scale * (1500 + 22 + 600 + 22 + 300); /* +AutoDec */ /* -AutoDec */ /* TODO: * - timer de match. */ /** Initialise behavior. */ static inline void taz_init (void) { taz_state = 0; taz_substate = 0; } /** Mise en place avant le départ. */ static inline void taz_state_0 (void) { switch (taz_substate) { case 0: /* Attend que l'on enfonce le jack. */ if (PINA & _BV (6)) taz_substate = 1; break; case 1: /* Attend que l'on enlève le jack. */ if (!(PINA & _BV (6))) { taz_substate = 2; /* FTW. */ motor_mode = 2; goto_mode = 10; goto_s = -0x10; goto_finish = 0; } break; case 2: if (goto_finish) { taz_substate = 3; /* Recalage. */ goto_y = taz_rear_32; goto_a = 0x00400000; /* On avance juste qu'à l'y de départ. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; goto_d = taz_start_y - taz_rear; goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; } break; case 3: if (goto_finish) { taz_substate = 4; /* Direction l'axe des x. */ motor_mode = 2; goto_mode = 1; goto_a = 0; goto_finish = 0; } break; case 4: if (goto_finish) { taz_substate = 5; /* FTW. */ motor_mode = 2; goto_mode = 10; goto_s = -0x10; goto_finish = 0; } break; case 5: if (goto_finish && (PINA & _BV (6))) { taz_substate = 6; /* Recalage. */ goto_x = taz_rear_32; goto_a = 0; } case 6: if (!(PINA & _BV (6))) { taz_state = 1; taz_substate = 0; } } } /** Avancée vers le pont. */ static inline void taz_state_1 (void) { switch (taz_substate) { case 0: break; } } /** Traversée du pont. */ static inline void taz_state_2 (void) { switch (taz_substate) { case 0: break; } } /** . */ static inline void taz_state_3 (void) { switch (taz_substate) { case 0: break; } } /** . */ static inline void taz_state_4 (void) { switch (taz_substate) { case 0: break; } } /** . */ static inline void taz_state_5 (void) { switch (taz_substate) { case 0: break; } } /** FSM. */ static inline void taz_update (void) { switch (taz_state) { case 0: taz_state_0 (); break; case 1: taz_state_1 (); break; case 2: taz_state_2 (); break; case 3: taz_state_3 (); break; case 4: taz_state_4 (); break; case 5: taz_state_5 (); break; } }