From 546699d6971f1b1cdb058473f6f2e455f94d11ff Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 1 Jun 2005 19:33:38 +0000 Subject: derniere version du dispositif anti blocage. --- n/asserv/src/taz.c | 36 ++++++++++++++++++++++++------------ 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index da5eff1..8c2fc5f 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -42,6 +42,7 @@ uint16_t taz_socle_min = 0x171; /** Anti blocage. */ uint8_t taz_bloc = 0; +uint16_t taz_bloc_counter; /** Positions. */ /******************************** @@ -496,7 +497,7 @@ taz_state_2 (void) taz_substate = 5; break; case 4: - taz_substate = 9; + taz_substate = taz_pont == 3 ? 95 : 9; /* Tour simple sur place. */ goto_angular (0xc0); break; @@ -522,7 +523,7 @@ taz_state_2 (void) case 8: if (goto_finish) { - taz_substate = 9; + taz_substate = taz_pont == 3 ? 95 : 9; goto_angular (0xc0); } break; @@ -534,6 +535,14 @@ taz_state_2 (void) 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; @@ -605,8 +614,8 @@ taz_state_2 (void) case 27: if (taz_dir_socle ()) { - taz_substate = 30; - goto_linear (450 * taz_scale); + taz_substate = 26; + motor_mode = 2; } break; @@ -623,7 +632,7 @@ taz_state_2 (void) { taz_substate = 32; /* Vers le pont. */ - goto_linear (((uint32_t) OR2 + 400 - (postrack_x >> 8)) * taz_scale); + goto_linear ((postrack_x >> 8) - ((uint32_t) OR2 + 200) * taz_scale); } break; case 32: @@ -639,7 +648,7 @@ taz_state_2 (void) { taz_substate = 34; /* Vers le milieu. */ - goto_linear (((uint32_t) 900 - 55 + (postrack_y >> 8)) * taz_scale); + goto_linear (((uint32_t) 900 + 75) * taz_scale + (postrack_y >> 8)); } break; case 34: @@ -655,7 +664,7 @@ taz_state_2 (void) { taz_substate = 36; /* Shoote les quilles. */ - goto_linear (300 * taz_scale); + goto_linear (200 * taz_scale); } break; case 36: @@ -663,7 +672,7 @@ taz_state_2 (void) { taz_substate = 40; /* Shoote les quilles. */ - goto_linear (-300 * taz_scale); + goto_linear (-200 * taz_scale); } break; @@ -764,21 +773,24 @@ taz_update (void) /* Blocages. */ if (taz_bloc) { - if (!goto_finish) + taz_bloc_counter--; + if (!goto_finish && taz_bloc_counter) { return; } proto_send1b ('b', 0); taz_bloc = 0; } - else 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)) + 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; - proto_send1b ('b', 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. */ -- cgit v1.2.3