From 990bf88df3c294f72e611789c467c8780ac6ced9 Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 28 Apr 2005 23:20:18 +0000 Subject: Modif dans goto_position. Nouvelles données pour les ponts. Amélioartion automate avant pont. --- n/asserv/src/goto.c | 31 +++++++++++--- n/asserv/src/taz.c | 120 +++++++++++++++++++++++++++++++++------------------- 2 files changed, 103 insertions(+), 48 deletions(-) diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index d833204..d35f9fa 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -111,8 +111,7 @@ goto_angular_mode (void) static inline void goto_position_mode (void) { - int32_t c, s; - /* Project in the robot base. */ + int32_t c, s, arc; goto_dx = goto_x - postrack_x; /* f24.8 */ goto_dy = goto_y - postrack_y; if (goto_dx < goto_eps && goto_dx > -goto_eps @@ -126,13 +125,35 @@ goto_position_mode (void) } else { + /* Project in the robot base. */ c = dsp_cos_f824 (postrack_a); s = dsp_sin_f824 (postrack_a); goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s); goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s); - /* Convert da into a arc. This is a rough aproximation. */ - goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8); - speed_distance (goto_dl / 2, goto_da * 2); + /* If very big angle (> 83 °), rotate. */ + if (goto_da > goto_dl * 8) + { + /* Compute arc. */ + arc = postrack_footing_2pi / 4; + speed_distance (0, goto_da > 0 ? arc : -arc); + } + /* If big angle (> 1.7°), rotate. */ + else if (goto_da * 32 > goto_dl) + { + /* Compute arc. This is a rough aproximation. */ + arc = goto_da / (goto_dl >> 8) * (postrack_footing / 2); + speed_distance (0, arc); + } + /* Is small angle (< 0.44°), strait ahead. */ + else if (goto_da * 128 < goto_dl) + { + speed_distance (goto_dl, 0); + } + /* Else, curve. TODO. */ + else + { + speed_distance (goto_dl, 0); + } } } diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index f615ed0..8aba939 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -59,28 +59,27 @@ static const uint16_t taz_after_bridge_16[4] = }; static const uint8_t taz_ang[4] = { - -18, + -19, -10, - 13, - -6 + 10, + -10 }; static const uint16_t taz_ang_dist_16[4] = { - taz_scale * (496), - taz_scale * (290), - taz_scale * (316), - taz_scale * (306) + taz_scale * (503), + taz_scale * (309), + taz_scale * (309), + taz_scale * (309) }; +/** Bridge position. */ uint8_t taz_pont; +/** Timer. */ +uint32_t taz_timer; /* +AutoDec */ /* -AutoDec */ -/* TODO: - * - timer de match. - */ - /** Initialise behavior. */ static inline void taz_init (void) @@ -172,6 +171,7 @@ taz_state_0 (void) { taz_state = 1; taz_substate = 0; + taz_timer = 0; } break; } @@ -198,7 +198,7 @@ taz_state_1 (void) if (goto_finish) { taz_substate = 2; - taz_tempo = 250; + taz_tempo = 225; } break; case 2: @@ -209,7 +209,6 @@ taz_state_1 (void) case 3: if (twi_master_is_finished ()) { - taz_substate = 4; proto_send3w ('H', motor_sharps[0], motor_sharps[1], motor_sharps[2]); /* Calcule la valeur du pont. */ @@ -227,14 +226,22 @@ taz_state_1 (void) taz_pont = 3; else { - taz_substate = 1; + taz_substate = 2; break; } /* Direction l'axe des y. */ - motor_mode = 2; - goto_mode = 1; - goto_a = 0xc0; - goto_finish = 0; + if (taz_pont == 0) + { + taz_substate = 10; + } + else + { + taz_substate = 6; + motor_mode = 2; + goto_mode = 1; + goto_a = 0xc0; + goto_finish = 0; + } } break; case 4: @@ -256,21 +263,27 @@ taz_state_1 (void) speed_restart (); postrack_y = -taz_rear_32; postrack_a = 0x00c00000; + } + // no break; + case 6: + if (goto_finish) + { + taz_substate = 7; /* On avance juste qu'à l'y de traversée de pont. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; - goto_d = taz_start_y_16 - taz_rear_16 + taz_pont * - taz_brige_interval_16; + goto_d = taz_start_y_16 + taz_pont * taz_brige_interval_16 - + (postrack_y >> 8); goto_x = postrack_x; goto_y = postrack_y; goto_finish = 0; } break; - case 6: + case 7: if (goto_finish) { - taz_substate = 7; + taz_substate = 8; /* Direction l'axe des x. */ motor_mode = 2; goto_mode = 1; @@ -278,32 +291,24 @@ taz_state_1 (void) goto_finish = 0; } break; - case 7: + case 8: if (goto_finish) { - taz_substate = 8; + taz_substate = 9; /* Reprend une mesure. */ twi_master_read (0x02, (uint8_t *) motor_sharps, 6); } break; - case 8: + case 9: if (twi_master_is_finished ()) { - taz_substate = 9; 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) { - /* Traversée du pont. */ - motor_mode = 2; - goto_mode = 0; - goto_sign = 0; - goto_d = taz_after_bridge_16[taz_pont] - (postrack_x >> 8); - goto_x = postrack_x; - goto_y = postrack_y; - goto_finish = 0; + taz_substate = 10; break; } else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117 @@ -336,17 +341,31 @@ taz_state_1 (void) taz_pont = 0; } /* Direction l'axe des y. */ + taz_substate = 4; motor_mode = 2; goto_mode = 1; goto_a = 0xc0; goto_finish = 0; - taz_substate = 4; } break; - case 9: + case 10: if (goto_finish) { - taz_substate = 10; + taz_substate = 11; + /* Traversée du pont. */ + motor_mode = 2; + goto_mode = 0; + goto_sign = 0; + goto_d = taz_after_bridge_16[taz_pont] - (postrack_x >> 8); + goto_x = postrack_x; + goto_y = postrack_y; + goto_finish = 0; + } + break; + case 11: + if (goto_finish) + { + taz_substate = 12; /* Tourne vers les quilles. */ motor_mode = 2; goto_mode = 1; @@ -354,11 +373,12 @@ taz_state_1 (void) goto_finish = 0; } break; - case 10: + case 12: if (goto_finish) { - taz_substate = 11; - /* Avance la petite distance, pour arriver sur la première quille. */ + taz_substate = 13; + /* Avance la petite distance, pour arriver sur la première + * quille. */ motor_mode = 2; goto_mode = 0; goto_sign = 0; @@ -368,10 +388,10 @@ taz_state_1 (void) goto_finish = 0; } break; - case 11: + case 13: if (goto_finish) { - taz_substate = 12; + taz_substate = 14; /* Tourne vers les quilles. */ motor_mode = 2; goto_mode = 1; @@ -382,7 +402,7 @@ taz_state_1 (void) } } -/** Traversée du pont. */ +/** . */ static void taz_state_2 (void) { @@ -433,14 +453,28 @@ taz_update (void) uint8_t old_state, old_substate; old_state = taz_state; old_substate = taz_substate; + /* Max state. */ if (taz_state > taz_max_state || (taz_state == taz_max_state && taz_substate > taz_max_substate)) return; + /* Pause timer. */ if (taz_tempo) { taz_tempo--; return; } + /* Game timer. */ +#if 0 + if (taz_state && taz_timer > 90 * 225) + { + taz_state = 0xff; + taz_substate = 0; + speed_restart (); + motor_mode = 0; + pwm_left = 0; + pwm_right = 0; + } +#endif switch (taz_state) { case 0: -- cgit v1.2.3