From 74550d3214bca333dcbcd73b75a1e7b535c7e1f0 Mon Sep 17 00:00:00 2001 From: schodet Date: Tue, 3 May 2005 01:26:58 +0000 Subject: Ajout d'une vitesse max l et a. Ajout des coefs l et r. Ajout de goto_counter. Ajout des fonctions de goto setup. --- n/asserv/src/taz.c | 99 ++++++++++++------------------------------------------ 1 file changed, 22 insertions(+), 77 deletions(-) (limited to 'n/asserv/src/taz.c') diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index 60c8330..913baa6 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -106,10 +106,7 @@ taz_state_0 (void) taz_substate = 2; /* FTW. */ speed_restart (); - motor_mode = 2; - goto_mode = 10; - goto_s = -10; - goto_finish = 0; + goto_ftw (-3); } break; case 2: @@ -121,13 +118,7 @@ taz_state_0 (void) 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; + goto_linear (taz_start_y_16 - taz_rear_16); } break; case 3: @@ -135,10 +126,7 @@ taz_state_0 (void) { taz_substate = 4; /* Direction l'axe des x. */ - motor_mode = 2; - goto_mode = 1; - goto_a = 0; - goto_finish = 0; + goto_angular (0); } break; case 4: @@ -146,10 +134,7 @@ taz_state_0 (void) { taz_substate = 5; /* FTW. */ - motor_mode = 2; - goto_mode = 10; - goto_s = -10; - goto_finish = 0; + goto_ftw (-3); } break; case 5: @@ -188,13 +173,7 @@ taz_state_1 (void) 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; + goto_linear (taz_before_bridge_16 - taz_rear_16); break; case 1: if (goto_finish) @@ -218,13 +197,13 @@ taz_state_1 (void) && motor_sharps[2] > 0x117) taz_pont = 0; else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 - && motor_sharps[2] > 0x117) + && motor_sharps[2] > 0x117) taz_pont = 1; else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] > 0x117) + && motor_sharps[2] > 0x117) taz_pont = 2; else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] < 0x117) + && motor_sharps[2] < 0x117) taz_pont = 3; else { @@ -233,16 +212,14 @@ taz_state_1 (void) } if (taz_pont == 0) { + /* Traversée directe. */ taz_substate = 10; } else { - /* Direction l'axe des y. */ taz_substate = 6; - motor_mode = 2; - goto_mode = 1; - goto_a = 0xc0; - goto_finish = 0; + /* Direction l'axe des y. */ + goto_angular (0xc0); } } break; @@ -251,10 +228,7 @@ taz_state_1 (void) { taz_substate = 5; /* FTW. */ - motor_mode = 2; - goto_mode = 10; - goto_s = -10; - goto_finish = 0; + goto_ftw (-3); } break; case 5: @@ -272,14 +246,9 @@ taz_state_1 (void) { taz_substate = 7; /* 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_pont * taz_brige_interval_16 + - (postrack_y >> 8); - goto_x = postrack_x; - goto_y = postrack_y; - goto_finish = 0; + goto_linear (taz_start_y_16 + + taz_pont * taz_brige_interval_16 + + (postrack_y >> 8)); } break; case 7: @@ -287,10 +256,7 @@ taz_state_1 (void) { taz_substate = 8; /* Direction l'axe des x. */ - motor_mode = 2; - goto_mode = 1; - goto_a = 0; - goto_finish = 0; + goto_angular (0); } break; case 8: @@ -342,12 +308,9 @@ taz_state_1 (void) { taz_pont = 0; } - /* Direction l'axe des y. */ taz_substate = 4; - motor_mode = 2; - goto_mode = 1; - goto_a = 0xc0; - goto_finish = 0; + /* Direction l'axe des y. */ + goto_angular (0xc0); } break; case 10: @@ -355,13 +318,7 @@ taz_state_1 (void) { taz_substate = 11; /* 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; + goto_linear (taz_after_bridge_16[taz_pont] - (postrack_x >> 8)); } break; case 11: @@ -369,10 +326,7 @@ taz_state_1 (void) { taz_substate = 12; /* Tourne vers les quilles. */ - motor_mode = 2; - goto_mode = 1; - goto_a = taz_ang[taz_pont]; - goto_finish = 0; + goto_angular (taz_ang[taz_pont]); } break; case 12: @@ -381,13 +335,7 @@ taz_state_1 (void) taz_substate = 13; /* 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; + goto_linear (taz_ang_dist_16[taz_pont]); } break; case 13: @@ -395,10 +343,7 @@ taz_state_1 (void) { taz_substate = 14; /* Tourne vers les quilles. */ - motor_mode = 2; - goto_mode = 1; - goto_a = 0; - goto_finish = 0; + goto_angular (0); } break; } -- cgit v1.2.3