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/goto.c | 7 ++ n/asserv/src/main.c | 13 ++++ n/asserv/src/taz.c | 212 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 232 insertions(+) create mode 100644 n/asserv/src/taz.c (limited to 'n/asserv') diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index 8aa5e91..c427af9 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -44,6 +44,8 @@ int8_t goto_s; int32_t goto_eps = 20L << 8; /** Debug values. */ int32_t goto_dx, goto_dy, goto_dl, goto_da; +/** Movement finished. */ +uint8_t goto_finish; /* +AutoDec */ /* -AutoDec */ @@ -65,6 +67,7 @@ goto_linear_mode (void) { speed_left_aim = 0; speed_right_aim = 0; + goto_finish = 1; } else { @@ -104,6 +107,7 @@ goto_position_mode (void) { speed_left_aim = 0; speed_right_aim = 0; + goto_finish = 1; } else { @@ -130,6 +134,7 @@ goto_position_exact_mode (void) { speed_left_aim = 0; speed_right_aim = 0; + goto_finish = 1; } else { @@ -166,6 +171,8 @@ goto_ftw_mode (void) speed_right = 0; speed_right_aim = 0; } + if (!(PINC & (_BV (0) || _BV (7)))) + goto_finish = 1; } /** Update the speed according to the desired destination. */ diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index e6fb72b..9eeca45 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -115,6 +115,10 @@ main_loop (void) if (motor_mode >= 2) { goto_update (); + if (goto_finish) + { + proto_send0 ('F'); + } } motor_timer_2 = timer_read (); /* Speed control. */ @@ -198,6 +202,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) if (goto_sign) goto_d = -goto_d; goto_x = postrack_x; goto_y = postrack_y; + goto_finish = 0; break; case c ('a', 1): /* Angular move. @@ -205,6 +210,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) motor_mode = 2; goto_mode = 1; goto_a = args[0]; + goto_finish = 0; break; case c ('g', 8): /* Go to position move. @@ -214,6 +220,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) goto_mode = 2; goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]); goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]); + goto_finish = 0; break; case c ('f', 1): /* Fuck the wall move. @@ -221,6 +228,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) motor_mode = 2; goto_mode = 10; goto_s = args[0]; + goto_finish = 0; + break; + case c ('F', 1): + /* Setup finish flag. + * - b: new value. */ + goto_finish = args[0]; break; case c ('s', 0): /* Set zero speed (stop). */ 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