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/goto.c | 2 +- n/asserv/src/main.c | 24 ++++++++--- n/asserv/src/taz.c | 122 ++++++++++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 138 insertions(+), 10 deletions(-) (limited to 'n') diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index fdc5953..552f853 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -107,7 +107,7 @@ goto_angular_mode (void) angle_diff <<= 8; angle_diff >>= 8; /* Small angles. */ - if (0x1000L > angle_diff && angle_diff > -0x1000L) + if (0x10000L > angle_diff && angle_diff > -0x10000L) { speed_left = 0; speed_right = 0; diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index ef607b3..d437f24 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -75,7 +75,7 @@ uint8_t motor_print_pinc, motor_print_pinc_cpt; /** Print Sharps. */ uint8_t motor_print_sharps, motor_print_sharps_cpt; -uint16_t motor_sharps[3]; +uint16_t motor_sharps[5]; /** Record timer value at different stage of computing. Used for performance * analisys. */ @@ -97,7 +97,7 @@ int main (void) { DDRD = 0x60; - PORTA = _BV (0) | _BV (7); + PORTA = _BV (7) | _BV (1) | _BV (0); pwm_init (); timer_init (); counter_init (); @@ -189,9 +189,10 @@ main_loop (void) { if (twi_master_is_finished ()) { - proto_send3w ('H', motor_sharps[0], motor_sharps[1], - motor_sharps[2]); - twi_master_read (0x02, (uint8_t *) motor_sharps, 6); + proto_send5w ('H', motor_sharps[0], motor_sharps[1], + motor_sharps[2], motor_sharps[3], + motor_sharps[4]); + twi_master_read (0x02, (uint8_t *) motor_sharps, 10); } motor_print_sharps_cpt = motor_print_sharps; } @@ -223,6 +224,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) taz_max_state = args[0]; taz_max_substate = args[1]; break; + case c ('t', 3): + taz_state = args[0]; + taz_substate = args[1]; + motor_taz = args[2]; + 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]); + taz_socles_right_min = v8_to_v16 (args[4], args[5]); + taz_socles_right_max = v8_to_v16 (args[6], args[7]); + break; case c ('l', 2): /* Linear move. * - w: distance (negative to go backward). */ @@ -316,7 +328,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('H', 1): /* Sharps stats. */ - twi_master_read (0x02, (uint8_t *) motor_sharps, 6); + twi_master_read (0x02, (uint8_t *) motor_sharps, 10); motor_print_sharps_cpt = motor_print_sharps = args[0]; break; default: 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