From b5172f235eacb732e838a401bf96cdeec9138f3b Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 5 May 2005 03:07:16 +0000 Subject: Modifs de tests de minuit chez Sopra. --- n/asserv/src/main.c | 11 ++- n/asserv/src/taz.c | 264 +++++++++++++++++++++++++++++++++++++++++----------- 2 files changed, 216 insertions(+), 59 deletions(-) diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index c0da1ec..0118540 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -78,7 +78,8 @@ uint8_t motor_print_pinc, motor_print_pinc_cpt; /** Print Sharps. */ uint8_t motor_print_sharps, motor_print_sharps_cpt; -uint16_t motor_sharps[5]; +uint8_t motor_print_sharps_no_send; +uint16_t motor_sharps[6]; /** Record timer value at different stage of computing. Used for performance * analisys. */ @@ -112,6 +113,7 @@ main (void) eeprom_read_params (); proto_send0 ('z'); sei (); + motor_taz = 1; main_loop (); return 0; } @@ -196,8 +198,9 @@ main_loop (void) { if (twi_master_is_finished ()) { - proto_send ('H', 10, (uint8_t *) motor_sharps); - twi_master_read (0x02, (uint8_t *) motor_sharps, 10); + if (!motor_print_sharps_no_send) + proto_send ('H', 12, (uint8_t *) motor_sharps); + twi_master_read (0x02, (uint8_t *) motor_sharps, 12); } motor_print_sharps_cpt = motor_print_sharps; } @@ -333,7 +336,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, 10); + twi_master_read (0x02, (uint8_t *) motor_sharps, 12); 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 1fcae4d..438c1fe 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -26,16 +26,19 @@ uint8_t taz_state; uint8_t taz_substate; /** FSM tempo. */ -uint8_t taz_tempo; +uint16_t taz_tempo; +static const uint16_t taz_1s = 900; /** FSM max state & substate. */ 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; +uint16_t taz_socles_right_min = 0x290, taz_socles_right_max = 0x2d8; /** Min pour un amas de quilles. */ uint16_t taz_quilles_min = 0x160; +/** Min pour un socle vu de haut. */ +uint16_t taz_socle_min = 0x171; /** Positions. */ /******************************** @@ -44,6 +47,7 @@ uint16_t taz_quilles_min = 0x160; * * ********************************/ #define taz_scale (1.0/.05077874252508530693) +#define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ static const uint32_t taz_rear_angle = 0x00ff90bf; static const uint16_t taz_rear_16 = taz_scale * 270 / 2; static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256; @@ -56,8 +60,8 @@ static const uint16_t taz_start_y_16 = taz_scale * 450 / 2; static const uint16_t taz_before_bridge_16 = taz_scale * 1200; static const uint16_t taz_brige_interval_16 = taz_scale * 150; -//#define OR2 (1500 + 22 + 600 + 22) -#define OR2 (1500 + 600 + 22) +#define OR2 (1500 + 22 + 600 + 22) +//#define OR2 (1500 + 600 + 22) static const uint16_t taz_after_bridge_16[4] = { taz_scale * (OR2), @@ -68,7 +72,7 @@ static const uint16_t taz_after_bridge_16[4] = static const uint8_t taz_ang[4] = { -19, - -10, + -12, 10, -10 }; @@ -109,7 +113,8 @@ taz_state_0 (void) /* Attend que l'on enlève le jack. */ if (PINA & _BV (6)) { - taz_substate = 2; + //taz_substate = 2; + taz_substate = 5; /* FTW. */ speed_restart (); goto_ftw (-3); @@ -151,6 +156,7 @@ taz_state_0 (void) speed_restart (); postrack_x = taz_rear_32; postrack_a = 0x00ff0000; + postrack_y = -taz_start_y_16; // } break; case 6: @@ -185,13 +191,13 @@ taz_state_1 (void) if (goto_finish) { taz_substate = 2; - taz_tempo = 225; + taz_tempo = taz_1s; } break; case 2: taz_substate = 3; /* Lance la capture. */ - twi_master_read (0x02, (uint8_t *) motor_sharps, 10); + twi_master_read (0x02, (uint8_t *) motor_sharps, 12); break; case 3: if (twi_master_is_finished ()) @@ -199,17 +205,17 @@ taz_state_1 (void) proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ - if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 - && motor_sharps[2] > 0x117) + if (motor_sharps[0] > 0x108 && motor_sharps[1] > 0x108 + && motor_sharps[2] > 0x108) taz_pont = 0; - else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 - && motor_sharps[2] > 0x117) + else if (motor_sharps[0] < 0x108 && motor_sharps[1] > 0x108 + && motor_sharps[2] > 0x108) taz_pont = 1; - else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] > 0x117) + else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 + && motor_sharps[2] > 0x108) taz_pont = 2; - else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] < 0x117) + else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 + && motor_sharps[2] < 0x108) taz_pont = 3; else { @@ -247,7 +253,7 @@ taz_state_1 (void) goto_linear (taz_start_y_16 + taz_pont * taz_brige_interval_16 + (postrack_y >> 8) - + (taz_pont == 0 ? 15 * taz_scale : 0)); + + (taz_pont < 2 ? 15 * taz_scale : 0)); } break; case 7: @@ -261,45 +267,49 @@ taz_state_1 (void) case 8: if (goto_finish) { - taz_substate = 9; - /* Reprend une mesure. */ - twi_master_read (0x02, (uint8_t *) motor_sharps, 10); + taz_substate = 85; + taz_tempo = taz_1s; } break; + case 85: + taz_substate = 9; + /* Reprend une mesure. */ + twi_master_read (0x02, (uint8_t *) motor_sharps, 12); + break; case 9: if (twi_master_is_finished ()) { proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ - if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 - && motor_sharps[2] > 0x117) + if (motor_sharps[0] > 0x108 && motor_sharps[1] > 0x108 + && motor_sharps[2] > 0x108) { taz_substate = 10; break; } - else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 - && motor_sharps[2] > 0x117 && taz_pont < 3) + else if (motor_sharps[0] < 0x108 && motor_sharps[1] > 0x108 + && motor_sharps[2] > 0x108 && taz_pont < 3) { taz_pont++; } - else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] > 0x117 && taz_pont < 2) + else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 + && motor_sharps[2] > 0x108 && taz_pont < 2) { taz_pont += 2; } - else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] < 0x117 && taz_pont < 1) + else if (motor_sharps[0] < 0x108 && motor_sharps[1] < 0x108 + && motor_sharps[2] < 0x108 && taz_pont < 1) { taz_pont += 3; } - else if (motor_sharps[0] > 0x117 && motor_sharps[1] < 0x117 - && motor_sharps[2] < 0x117 && taz_pont > 1) + else if (motor_sharps[0] > 0x108 && motor_sharps[1] < 0x108 + && motor_sharps[2] < 0x108 && taz_pont > 1) { taz_pont -= 2; } - else if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117 - && motor_sharps[2] < 0x117 && taz_pont > 0) + else if (motor_sharps[0] > 0x108 && motor_sharps[1] > 0x108 + && motor_sharps[2] < 0x108 && taz_pont > 0) { taz_pont--; } @@ -343,6 +353,18 @@ taz_state_1 (void) taz_substate = 14; /* Tourne vers les quilles. */ goto_angular (0); + /* Démare les sharps. */ + twi_master_read (0x02, (uint8_t *) motor_sharps, 12); + motor_print_sharps_cpt = 1; + motor_print_sharps = 8; + motor_print_sharps_no_send = 1; + } + break; + case 14: + if (goto_finish) + { + taz_state = 2; + taz_substate = 0; } break; } @@ -416,18 +438,28 @@ taz_dir_socle (void) { if (left) { - speed_left_aim = -2; - speed_right_aim = 2; + speed_left_aim = -1; + speed_right_aim = 1; } else { - speed_left_aim = 2; - speed_right_aim = -2; + speed_left_aim = 1; + speed_right_aim = -1; } } return 0; } +/** Test si socles. */ +static uint8_t +taz_test_socles (void) +{ + return (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); +} + /** Où sont les quilles !. */ static void taz_state_2 (void) @@ -436,33 +468,156 @@ taz_state_2 (void) switch (taz_substate) { case 0: - /* Trouve un tas de quille. */ - r = taz_find_quilles (0x80, 1); - if (r == 1) - taz_substate = 1; + taz_substate = 1; + /* Tout droit. */ + goto_linear (600 * taz_scale); 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; + else if (taz_test_socles ()) + taz_substate = 2; break; - case 3: + case 2: if (taz_dir_socle ()) { - taz_substate = 2; + taz_substate = 1; motor_mode = 2; } break; + case 3: + /* Sur un socle ? */ + proto_send1w ('H', motor_sharps[5]); + if (motor_sharps[5] > taz_socle_min) + taz_substate = 4; + else + taz_substate = 5; + break; case 4: + taz_substate = 9; + /* Tour simple sur place. */ + goto_angular (0xc0); + break; + case 5: + taz_substate++; + /* Spécial move sans socle. */ + goto_linear (150 * taz_scale); + break; + case 6: + if (goto_finish) + { + taz_substate++; + goto_angular (0xe0); + } + break; + case 7: + if (goto_finish) + { + taz_substate++; + goto_linear (-150 * M_SQRT2 * taz_scale); + } + break; + case 8: + if (goto_finish) + { + taz_substate = 9; + goto_angular (0xc0); + } + break; + case 9: + if (goto_finish) + { + taz_substate = 10; + /* Tout droit sur la rangée du fond. */ + goto_linear ((1500 - 150) * taz_scale + (postrack_y >> 8)); + } + break; + case 10: + if (goto_finish) + taz_substate = 20; + else if (taz_test_socles ()) + taz_substate = 11; + break; + case 11: + if (taz_dir_socle ()) + { + taz_substate = 10; + motor_mode = 2; + } + break; + + case 20: + /* Direction en point de charge !!! */ + if (motor_sharps[5] > taz_socle_min) + { + taz_substate = 21; + goto_angular (115); + } + else + { + taz_substate = 22; + goto_linear (150 * taz_scale); + } + break; + case 21: + if (goto_finish) + { + taz_substate = 24; + goto_linear (474 * taz_scale); + } + break; + case 22: + if (goto_finish) + { + taz_substate = 23; + goto_angular (0x80); + } + break; + case 23: + if (goto_finish) + { + taz_substate = 24; + goto_linear (450 * taz_scale); + } + break; + case 24: + if (goto_finish) + { + taz_substate = 25; + goto_angular (0x40); + } + break; + case 25: + if (goto_finish) + { + taz_substate = 26; + goto_linear (1200 * taz_scale); + } + break; + case 26: + if (goto_finish) + taz_substate = 28; + else if (taz_test_socles ()) + taz_substate = 27; + break; + case 27: + if (taz_dir_socle ()) + { + taz_substate = 28; + goto_linear (600 * taz_scale); + } + break; + case 28: + if (goto_finish) + { + uint8_t a = v32_to_v8 (postrack_a, 2) + 0xf0; + r = taz_find_quilles (a, 0); + if (r == 1) + { + taz_substate = 26; + goto_linear (600 * taz_scale); + } + } break; } } @@ -519,7 +674,7 @@ taz_update (void) } /* Game timer. */ #if 0 - if (taz_state && taz_timer > 90 * 225) + if (taz_state && taz_timer > 90 * taz_1s) { taz_state = 0xff; taz_substate = 0; @@ -538,8 +693,7 @@ taz_update (void) taz_state_1 (); break; case 2: - motor_print_sharps = 8; - speed_max_l = 3; + speed_max_l = 5; taz_state_2 (); break; case 3: -- cgit v1.2.3