From 123f059880d7d9ebd0ed25768d7b41a07e0280f1 Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 4 May 2005 07:51:13 +0000 Subject: Support des socles. --- n/asserv/src/taz.c | 122 +++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 119 insertions(+), 3 deletions(-) (limited to 'n/asserv/src/taz.c') diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index 913baa6..468e4ed 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -31,6 +31,12 @@ uint8_t taz_tempo; 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. */ /******************************** * * @@ -185,7 +191,7 @@ taz_state_1 (void) case 2: taz_substate = 3; /* Lance la capture. */ - twi_master_read (0x02, (uint8_t *) motor_sharps, 6); + twi_master_read (0x02, (uint8_t *) motor_sharps, 10); break; case 3: if (twi_master_is_finished ()) @@ -264,7 +270,7 @@ taz_state_1 (void) { taz_substate = 9; /* Reprend une mesure. */ - twi_master_read (0x02, (uint8_t *) motor_sharps, 6); + twi_master_read (0x02, (uint8_t *) motor_sharps, 10); } break; case 9: @@ -349,13 +355,121 @@ taz_state_1 (void) } } -/** . */ +/** 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; } } @@ -431,6 +545,8 @@ taz_update (void) taz_state_1 (); break; case 2: + motor_print_sharps = 8; + speed_max_l = 3; taz_state_2 (); break; case 3: -- cgit v1.2.3