summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2005-04-28 23:20:18 +0000
committerschodet2005-04-28 23:20:18 +0000
commit990bf88df3c294f72e611789c467c8780ac6ced9 (patch)
tree57003283e2fc9c376bb256e496dc3615cf538022
parentc5b9c0e49bbb2c06f0871275c6a15150c3d9d41c (diff)
Modif dans goto_position.
Nouvelles données pour les ponts. Amélioartion automate avant pont.
-rw-r--r--n/asserv/src/goto.c31
-rw-r--r--n/asserv/src/taz.c120
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: