/* 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; uint8_t taz_tempo; /** Positions. */ //#define taz_scale (1.0/0.05026548245743669181) //#define taz_scale (1.0/.05090591755319148936) #define taz_scale (1.0/.05132374757503138194) 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 - 50) / 2; static const uint16_t taz_before_bridge_16 = taz_scale * 1200; static const uint16_t taz_brige_interval_16 = taz_scale * 140; #define OR2 (1500 + 22 + 600 + 22) static const uint16_t taz_after_bridge_16[4] = { taz_scale * (OR2), taz_scale * (OR2 + 150), taz_scale * (OR2 + 150), taz_scale * (OR2 + 150) }; static const uint8_t taz_ang[4] = { -18, -10, 13, -6 }; static const uint16_t taz_ang_dist_16[4] = { taz_scale * (496), taz_scale * (290), taz_scale * (316), taz_scale * (306) }; uint8_t taz_pont; /* +AutoDec */ /* -AutoDec */ /* TODO: * - timer de match. */ /** Initialise behavior. */ static inline void taz_init (void) { } /** Mise en place avant le départ. */ static 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 = -10; goto_finish = 0; } break; case 2: if (goto_finish) { taz_substate = 3; /* Recalage. */ speed_restart (); postrack_y = -taz_rear_32; postrack_a = 0x00c00000; /* On avance juste qu'à l'y de départ. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; goto_d = taz_start_y_16 - taz_rear_16; 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 = -10; goto_finish = 0; } break; case 5: if (goto_finish) { taz_substate = 6; /* Recalage. */ speed_restart (); postrack_x = taz_rear_32; postrack_a = 0; } break; case 6: if (!(PINA & _BV (6))) { taz_substate = 7; } break; case 7: if (PINA & _BV (6)) { taz_state = 1; taz_substate = 0; } break; } } /** Avancée vers le pont. */ static void taz_state_1 (void) { switch (taz_substate) { case 0: taz_substate = 1; /* On avance juste qu'au pont. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; goto_d = taz_before_bridge_16 - taz_rear_16; goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; break; case 1: if (goto_finish) { taz_substate = 2; taz_tempo = 250; } break; case 2: taz_substate = 3; /* Lance la capture. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 6); break; case 3: if (twi_master_is_finished ()) { taz_substate = 4; proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 && motor_sharps[2] > 0x117) taz_pont = 0; else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 && motor_sharps[2] > 0x117) taz_pont = 1; else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 && motor_sharps[2] > 0x117) taz_pont = 2; else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 && motor_sharps[2] < 0x117) taz_pont = 3; else { taz_substate = 1; break; } /* Direction l'axe des y. */ motor_mode = 2; goto_mode = 1; goto_a = 0xc0; goto_finish = 0; } break; case 4: if (goto_finish) { taz_substate = 5; /* FTW. */ motor_mode = 2; goto_mode = 10; goto_s = -10; goto_finish = 0; } break; case 5: if (goto_finish) { taz_substate = 6; /* Recalage. */ speed_restart (); postrack_y = taz_rear_32; postrack_a = 0x00c00000; /* On avance juste qu'à l'y de traversée de pont. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; goto_d = taz_start_y_16 - taz_rear_16 + taz_pont * taz_brige_interval_16; goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; } break; case 6: if (goto_finish) { taz_substate = 7; /* Direction l'axe des x. */ motor_mode = 2; goto_mode = 1; goto_a = 0; goto_finish = 0; } break; case 7: if (goto_finish) { taz_substate = 8; /* Reprend une mesure. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 6); } break; case 8: if (twi_master_is_finished ()) { taz_substate = 9; proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 && motor_sharps[2] > 0x117) { /* Traversée du pont. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; goto_d = taz_after_bridge_16[taz_pont] - (postrack_x >> 8); goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; break; } else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 && motor_sharps[2] > 0x117 && taz_pont < 3) { taz_pont++; } else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 && motor_sharps[2] > 0x117 && taz_pont < 2) { taz_pont += 2; } else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 && motor_sharps[2] < 0x117 && taz_pont < 1) { taz_pont += 3; } else if (motor_sharps[0] > 0x117 && motor_sharps[1] < 0x117 && motor_sharps[2] < 0x117 && taz_pont > 1) { taz_pont -= 2; } else if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 && motor_sharps[2] < 0x117 && taz_pont > 0) { taz_pont--; } else { taz_pont = 0; } /* Direction l'axe des y. */ motor_mode = 2; goto_mode = 1; goto_a = 0xc0; goto_finish = 0; taz_substate = 4; } break; case 9: if (goto_finish) { taz_substate = 10; /* Tourne vers les quilles. */ motor_mode = 2; goto_mode = 1; goto_a = taz_ang[taz_pont]; goto_finish = 0; } break; case 10: if (goto_finish) { taz_substate = 11; /* Avance la petite distance, pour arriver sur la première quille. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; goto_d = taz_ang_dist_16[taz_pont]; goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; } break; case 11: if (goto_finish) { taz_substate = 12; /* Tourne vers les quilles. */ motor_mode = 2; goto_mode = 1; goto_a = 0; goto_finish = 0; } break; } } /** Traversée du pont. */ static void taz_state_2 (void) { switch (taz_substate) { case 0: break; } } /** . */ static void taz_state_3 (void) { switch (taz_substate) { case 0: break; } } /** . */ static void taz_state_4 (void) { switch (taz_substate) { case 0: break; } } /** . */ static void taz_state_5 (void) { switch (taz_substate) { case 0: break; } } /** FSM. */ static void taz_update (void) { uint8_t old_state, old_substate; old_state = taz_state; old_substate = taz_substate; if (taz_tempo) { taz_tempo--; return; } 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; } if (old_state != taz_state || old_substate != taz_substate) proto_send2b ('t', taz_state, taz_substate); }