From b37a11a2f303519ffb59e96dd9a853b1945df45b Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 16 Apr 2005 14:36:22 +0000 Subject: Première version de taz FSM. --- n/asserv/src/taz.c | 212 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100644 n/asserv/src/taz.c (limited to 'n/asserv/src/taz.c') diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c new file mode 100644 index 0000000..b7e941d --- /dev/null +++ b/n/asserv/src/taz.c @@ -0,0 +1,212 @@ +/* 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_scale * 270 / 2 * 256; +static const uint16_t taz_front_16 = taz_scale * 270 / 2; +static const uint32_t taz_front_32 = taz_scale * 270 / 2 * 256; +static const uint16_t taz_side_16 = taz_scale * 340 / 2; +static const uint32_t taz_side_32 = taz_scale * 340 / 2 * 256; +static const uint16_t taz_start_y_16 = taz_scale * 450 / 2; + +/* +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 (PINC & _BV (3)) + taz_substate = 1; + break; + case 1: + /* Attend que l'on enlève le jack. */ + if (!(PINC & _BV (3))) + { + 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 && (PINC & _BV (3))) + { + taz_substate = 6; + /* Recalage. */ + goto_x = taz_rear_32; + goto_a = 0; + } + case 6: + if (!(PINC & _BV (3))) + { + 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; + } +} -- cgit v1.2.3