/* 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. */ uint8_t taz_tempo; /** 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 = 0x2d0; /** Min pour un amas de quilles. */ uint16_t taz_quilles_min = 0x160; /** Positions. */ /******************************** * * * !!! LES Y SONT NÉGATIFS !!! * * * ********************************/ #define taz_scale (1.0/.05077874252508530693) 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) 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, -10, 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; /* +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; /* 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 = 0x00c00000; /* 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 = 0; } break; case 6: if (!(PINA & _BV (6))) { taz_substate = 7; } break; case 7: if (PINA & _BV (6)) { taz_state = 1; taz_substate = 0; taz_timer = 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 = 225; } break; case 2: taz_substate = 3; /* Lance la capture. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 10); 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] > 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 = 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 = 0x00c00000; } // 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)); } 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 = 9; /* Reprend une mesure. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 10); } 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] > 0x117 && motor_sharps[1] > 0x117 && motor_sharps[2] > 0x117) { taz_substate = 10; 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; } 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); } 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 = -2; speed_right_aim = 2; } else { speed_left_aim = 2; speed_right_aim = -2; } } return 0; } /** Où sont les quilles !. */ static void taz_state_2 (void) { uint8_t r; switch (taz_substate) { case 0: /* Trouve un tas de quille. */ r = taz_find_quilles (0x80, 1); if (r == 1) taz_substate = 1; break; case 1: taz_substate = 2; /* Va vers le tas de quille. */ goto_linear (300 * taz_scale); break; case 2: if (goto_finish) taz_substate = 4; else if ((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)) taz_substate = 3; break; case 3: if (taz_dir_socle ()) { taz_substate = 2; motor_mode = 2; } break; case 4: 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; /* 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; } /* Game timer. */ #if 0 if (taz_state && taz_timer > 90 * 225) { taz_state = 0xff; taz_substate = 0; speed_restart (); motor_mode = 0; pwm_left = 0; pwm_right = 0; } #endif switch (taz_state) { case 0: taz_state_0 (); break; case 1: taz_state_1 (); break; case 2: motor_print_sharps = 8; speed_max_l = 3; 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); }