From 03f151f322ddcf22796b810a8a2cb02b55ede491 Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 6 May 2005 14:13:39 +0000 Subject: Match 3. --- n/asserv/src/goto.c | 4 ++-- n/asserv/src/main.c | 3 +++ n/asserv/src/taz.c | 55 +++++++++++++++++++++++++++++++++++++++++------------ 3 files changed, 48 insertions(+), 14 deletions(-) (limited to 'n/asserv') diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index 07ab840..1d8dffb 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -254,8 +254,8 @@ goto_counter_mode (void) int32_t dl248, dr248; dl = goto_counter_left - counter_left; dr = goto_counter_right - counter_right; - if (dl < 3 && dl > -3 - && dr < 3 && dr > -3) + if (dl < 20 && dl > -20 + && dr < 20 && dr > -20) { if (!goto_finish) goto_finish = 1; diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 0118540..b944f8a 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -237,6 +237,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) taz_substate = args[1]; motor_taz = args[2]; break; + case c ('T', 0): + proto_send1d ('T', taz_timer); + break; case c ('o', 8): taz_socles_left_min = v8_to_v16 (args[0], args[1]); taz_socles_left_max = v8_to_v16 (args[2], args[3]); diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index aa7b9c3..533f634 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -27,7 +27,7 @@ uint8_t taz_state; uint8_t taz_substate; /** FSM tempo. */ uint16_t taz_tempo; -static const uint16_t taz_1s = 900; +#define taz_1s 900 /** FSM max state & substate. */ uint8_t taz_max_state = 0xff; uint8_t taz_max_substate; @@ -40,6 +40,9 @@ 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; + /** Positions. */ /******************************** * * @@ -87,7 +90,7 @@ static const uint16_t taz_ang_dist_16[4] = /** Bridge position. */ uint8_t taz_pont; /** Timer. */ -uint32_t taz_timer; +uint32_t taz_timer = 90L * taz_1s; /* +AutoDec */ /* -AutoDec */ @@ -170,7 +173,6 @@ taz_state_0 (void) { taz_state = 1; taz_substate = 0; - taz_timer = 0; } break; } @@ -604,10 +606,17 @@ taz_state_2 (void) if (taz_dir_socle ()) { taz_substate = 28; - goto_linear (600 * taz_scale); + goto_linear (450 * taz_scale); } break; case 28: + if (goto_finish) + { + taz_substate = 29; + goto_angular (0x60); + } + break; + case 29: if (goto_finish) { uint8_t a = v32_to_v8 (postrack_a, 2) + 0xf0; @@ -663,16 +672,38 @@ taz_update (void) old_state = taz_state; old_substate = taz_substate; /* Game timer. */ - taz_timer++; - if (taz_state && taz_timer > 4 * 90 * taz_1s) + 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) + { + if (!goto_finish) + { + 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)) { - taz_state = 0xff; - taz_substate = 0; speed_restart (); - motor_mode = 0; - pwm_left = 0; - pwm_right = 0; - proto_send2b ('t', 0xff, 0xff); + goto_counter (-70 * taz_scale, -100 * taz_scale); + taz_bloc = 1; + proto_send1b ('b', 1); + return; } /* Max state. */ if (taz_state > taz_max_state -- cgit v1.2.3