/* 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 & substate. */ uint8_t taz_state; uint8_t taz_substate; /** FSM tempo. */ uint16_t taz_tempo; #define taz_1s 900 /** FSM max state & substate. */ uint8_t taz_max_state = 0xff; uint8_t taz_max_substate; /** Min et max pour les socles. */ uint16_t taz_socles_left_min = 0x220, taz_socles_left_max = 0x270; uint16_t taz_socles_right_min = 0x290, taz_socles_right_max = 0x2d8; /** Min pour un amas de quilles. */ uint16_t taz_quilles_min = 0x160; /** Min pour un socle vu de haut. */ uint16_t taz_socle_min = 0x171; /** Anti blocage. */ uint8_t taz_bloc = 0; uint16_t taz_bloc_counter; /** Positions. */ /******************************** * * * !!! LES Y SONT NÉGATIFS !!! * * * ********************************/ #define taz_scale (1.0/.05077874252508530693) #define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ static const uint32_t taz_rear_angle = 0x00ff90bf; 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; static const uint16_t taz_before_bridge_16 = taz_scale * 1200; static const uint16_t taz_brige_interval_16 = taz_scale * 150; #define OR2 (1500 + 22 + 600 + 22) //#define OR2 (1500 + 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] = { -19, -12, 10, -10 }; static const uint16_t taz_ang_dist_16[4] = { taz_scale * (503), taz_scale * (309), taz_scale * (309), taz_scale * (309) }; /** Bridge position. */ uint8_t taz_pont; /** Timer. */ uint32_t taz_timer = 90L * taz_1s; /* +AutoDec */ /* -AutoDec */ /** 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; taz_substate = 5; /* FTW. */ speed_restart (); goto_ftw (-3); } break; case 2: if (goto_finish) { taz_substate = 3; /* Recalage. */ speed_restart (); postrack_y = -taz_rear_32; postrack_a = 0x00bf0000; /* On avance juste qu'à l'y de départ. */ goto_linear (taz_start_y_16 - taz_rear_16); } break; case 3: if (goto_finish) { taz_substate = 4; /* Direction l'axe des x. */ goto_angular (0); } break; case 4: if (goto_finish) { taz_substate = 5; /* FTW. */ goto_ftw (-3); } break; case 5: if (goto_finish) { taz_substate = 6; /* Recalage. */ speed_restart (); postrack_x = taz_rear_32; postrack_a = 0x00ff0000; postrack_y = -taz_start_y_16; // } 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. */ goto_linear (taz_before_bridge_16 - taz_rear_16); break; case 1: if (goto_finish) { taz_substate = 2; taz_tempo = taz_1s; } break; case 2: taz_substate = 3; /* Lance la capture. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 12); break; case 3: if (twi_master_is_finished ()) { proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ if (motor_sharps[0] > 0x108 && motor_sharps[1] > 0x108 && motor_sharps[2] > 0x108) taz_pont = 0; else if (motor_sharps[0] < 0x108 && motor_sharps[1] > 0x108 && motor_sharps[2] > 0x108) taz_pont = 1; else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 && motor_sharps[2] > 0x108) taz_pont = 2; else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 && motor_sharps[2] < 0x108) taz_pont = 3; else { taz_substate = 2; break; } taz_substate = 4; /* Direction l'axe des y. */ goto_angular (0xc0); } break; case 4: if (goto_finish) { taz_substate = 5; /* FTW. */ goto_ftw (-3); } break; case 5: if (goto_finish) { taz_substate = 6; /* Recalage. */ speed_restart (); postrack_y = -taz_rear_32; postrack_a = 0x00be0000; } // no break; case 6: if (goto_finish) { taz_substate = 7; /* On avance juste qu'à l'y de traversée de pont. */ goto_linear (taz_start_y_16 + taz_pont * taz_brige_interval_16 + (postrack_y >> 8) + (taz_pont < 2 ? 15 * taz_scale : 0)); } break; case 7: if (goto_finish) { taz_substate = 8; /* Direction l'axe des x. */ goto_angular (0); } break; case 8: if (goto_finish) { taz_substate = 85; taz_tempo = taz_1s; } break; case 85: taz_substate = 9; /* Reprend une mesure. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 12); break; case 9: if (twi_master_is_finished ()) { proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ if (motor_sharps[0] > 0x108 && motor_sharps[1] > 0x108 && motor_sharps[2] > 0x108) { taz_substate = 10; break; } else if (motor_sharps[0] < 0x108 && motor_sharps[1] > 0x108 && motor_sharps[2] > 0x108 && taz_pont < 3) { taz_pont++; } else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 && motor_sharps[2] > 0x108 && taz_pont < 2) { taz_pont += 2; } else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 && motor_sharps[2] < 0x108 && taz_pont < 1) { taz_pont += 3; } else if (motor_sharps[0] > 0x108 && motor_sharps[1] < 0x108 && motor_sharps[2] < 0x108 && taz_pont > 1) { taz_pont -= 2; } else if (motor_sharps[0] > 0x108 && motor_sharps[1] > 0x108 && motor_sharps[2] < 0x108 && taz_pont > 0) { taz_pont--; } else { taz_pont = 0; } taz_substate = 4; /* Direction l'axe des y. */ goto_angular (0xc0); } break; case 10: if (goto_finish) { taz_substate = 11; /* Traversée du pont. */ goto_linear (taz_after_bridge_16[taz_pont] - (postrack_x >> 8)); } break; case 11: if (goto_finish) { taz_substate = 12; /* Tourne vers les quilles. */ goto_angular (taz_ang[taz_pont]); } break; case 12: if (goto_finish) { taz_substate = 13; /* Avance la petite distance, pour arriver sur la première * quille. */ goto_linear (taz_ang_dist_16[taz_pont]); } break; case 13: if (goto_finish) { taz_substate = 14; /* Tourne vers les quilles. */ goto_angular (0); /* Démare les sharps. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 12); motor_print_sharps_cpt = 1; motor_print_sharps = 4; motor_print_sharps_no_send = 1; } break; case 14: if (goto_finish) { taz_state = 2; taz_substate = 0; } break; } } /** Trouve un amas de quilles sur un certain angle et sens, retourne 1 si * ok, 2 si angle fini. */ static uint8_t taz_find_quilles (uint8_t a, uint8_t dir) { uint32_t angle_diff; /* Compute angle diff. */ angle_diff = v8_to_v32 (0, goto_a, 0, 0) - postrack_a; angle_diff <<= 8; angle_diff >>= 8; motor_mode = 1; /* Small angles. */ if (0x10000L > angle_diff && angle_diff > -0x10000L) { speed_left = 0; speed_right = 0; speed_left_aim = 0; speed_right_aim = 0; return 2; } else if (motor_sharps[1] > taz_quilles_min) { speed_left_aim = 0; speed_right_aim = 0; return 1; } else { if (dir) { speed_left_aim = 2; speed_right_aim = -2; } else { speed_left_aim = -2; speed_right_aim = 2; } return 0; } } /** Se tourne vers un socle, retourne 1 si ok. */ static uint8_t taz_dir_socle (void) { uint8_t left, right; left = motor_sharps[3] > taz_socles_left_min && motor_sharps[3] < taz_socles_left_max; right = motor_sharps[4] > taz_socles_right_min && motor_sharps[4] < taz_socles_right_max; motor_mode = 1; if (left == right) { if (speed_left == 0 && speed_right == 0) { return 1; } else { speed_left_aim = 0; speed_right_aim = 0; } } else { if (left) { speed_left_aim = -1; speed_right_aim = 1; } else { speed_left_aim = 1; speed_right_aim = -1; } } return 0; } /** Test si socles. */ static uint8_t taz_test_socles (void) { return (motor_sharps[3] > taz_socles_left_min && motor_sharps[3] < taz_socles_left_max) || (motor_sharps[4] > taz_socles_right_min && motor_sharps[4] < taz_socles_right_max); } /** Où sont les quilles !. */ static void taz_state_2 (void) { uint8_t r; switch (taz_substate) { case 0: taz_substate = 1; /* Tout droit. */ goto_linear (600 * taz_scale); break; case 1: if (goto_finish) taz_substate = 3; else if (taz_test_socles ()) taz_substate = 2; break; case 2: if (taz_dir_socle ()) { taz_substate = 1; motor_mode = 2; } break; case 3: /* Sur un socle ? */ proto_send1w ('H', motor_sharps[5]); if (motor_sharps[5] > taz_socle_min) taz_substate = 4; else taz_substate = 5; break; case 4: taz_substate = taz_pont == 3 ? 95 : 9; /* Tour simple sur place. */ goto_angular (0xc0); break; case 5: taz_substate++; /* Spécial move sans socle. */ goto_linear (150 * taz_scale); break; case 6: if (goto_finish) { taz_substate++; goto_angular (0xe0); } break; case 7: if (goto_finish) { taz_substate++; goto_linear (-150 * M_SQRT2 * taz_scale); } break; case 8: if (goto_finish) { taz_substate = taz_pont == 3 ? 95 : 9; goto_angular (0xc0); } break; case 9: if (goto_finish) { taz_substate = 10; /* Tout droit sur la rangée du fond. */ goto_linear ((1500 - 150) * taz_scale + (postrack_y >> 8)); } break; case 95: if (goto_finish) { taz_substate = 9; /* Marche arrière toute. */ goto_linear ((450 - 50 - 270 / 2) * taz_scale + (postrack_y >> 8)); } break; case 10: if (goto_finish) taz_substate = 20; else if (taz_test_socles ()) taz_substate = 11; break; case 11: if (taz_dir_socle ()) { taz_substate = 10; motor_mode = 2; } break; case 20: /* Direction en point de charge !!! */ if (motor_sharps[5] > taz_socle_min) { taz_substate = 21; goto_angular (115); } else { taz_substate = 22; goto_linear (150 * taz_scale); } break; case 21: if (goto_finish) { taz_substate = 24; goto_linear (474 * taz_scale); } break; case 22: if (goto_finish) { taz_substate = 23; goto_angular (0x80); } break; case 23: if (goto_finish) { taz_substate = 24; goto_linear (450 * taz_scale); } break; case 24: if (goto_finish) { taz_substate = 25; goto_angular (0x40); } break; case 25: if (goto_finish) { taz_substate = 26; goto_linear (1200 * taz_scale); } break; case 26: if (goto_finish) taz_substate = 30; else if (taz_test_socles ()) taz_substate = 27; break; case 27: if (taz_dir_socle ()) { taz_substate = 26; motor_mode = 2; } break; case 30: if (goto_finish) { taz_substate = 31; /* Direction notre camps pour shooter les quilles gratuites. */ goto_angular (0x80); } break; case 31: if (goto_finish) { taz_substate = 32; /* Vers le pont. */ goto_linear ((postrack_x >> 8) - ((uint32_t) OR2 + 200) * taz_scale); } break; case 32: if (goto_finish) { taz_substate = 33; /* 90° */ goto_angular (0xc0); } break; case 33: if (goto_finish) { taz_substate = 34; /* Vers le milieu. */ goto_linear (((uint32_t) 900 + 75) * taz_scale + (postrack_y >> 8)); } break; case 34: if (goto_finish) { taz_substate = 35; /* Direction les quilles. */ goto_angular (0x80); } break; case 35: if (goto_finish) { taz_substate = 36; /* Shoote les quilles. */ goto_linear (200 * taz_scale); } break; case 36: if (goto_finish) { taz_substate = 40; /* Shoote les quilles. */ goto_linear (-200 * taz_scale); } break; case 40: if (goto_finish) { taz_substate = 41; goto_angular (0x20); } break; case 41: if (goto_finish) { r = taz_find_quilles (0x40, 0); if (r) { taz_substate = 42; goto_linear (600 * taz_scale); } } break; case 42: if (goto_finish) { taz_substate = 43; goto_angular (0x20); } break; case 43: if (goto_finish) { r = taz_find_quilles (0xe0, 1); if (r) { taz_substate = 44; goto_linear (600 * taz_scale); } } 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. */ void taz_update (void) { uint8_t old_state, old_substate; old_state = taz_state; old_substate = taz_substate; /* Game timer. */ if (taz_state) { taz_timer--; if (!taz_timer) { taz_state = 0xff; taz_substate = 0xff; speed_restart (); motor_mode = 0; pwm_left = 0; pwm_right = 0; proto_send2b ('t', 0xff, 0xff); } } /* Blocages. */ if (taz_bloc) { taz_bloc_counter--; if (!goto_finish && taz_bloc_counter) { return; } proto_send1b ('b', 0); taz_bloc = 0; } if ((speed_left_e_old == speed_e_sat || speed_left_e_old == -speed_e_sat) && (speed_right_e_old == speed_e_sat || speed_right_e_old == -speed_e_sat)) { speed_restart (); goto_counter (speed_left_e_old == speed_e_sat ? -70 * taz_scale : 70 * taz_scale, speed_right_e_old == speed_e_sat ? -100 * taz_scale : 100 * taz_scale); taz_bloc = 1; taz_bloc_counter = 3 * taz_1s; proto_send3b ('b', 1, speed_left_e_old == speed_e_sat, speed_right_e_old == speed_e_sat); return; } /* Max state. */ if (taz_state > taz_max_state || (taz_state == taz_max_state && taz_substate > taz_max_substate)) return; /* Pause timer. */ if (taz_tempo) { taz_tempo--; return; } switch (taz_state) { case 0: taz_state_0 (); break; case 1: taz_state_1 (); break; case 2: speed_max_l = 5; 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); }