summaryrefslogtreecommitdiff
path: root/n/asserv/src/taz.c
diff options
context:
space:
mode:
authorschodet2005-05-04 07:51:13 +0000
committerschodet2005-05-04 07:51:13 +0000
commit123f059880d7d9ebd0ed25768d7b41a07e0280f1 (patch)
treed450c47ddc0165a5fb924515596f171b5a97f05b /n/asserv/src/taz.c
parentb9f9bac0d99ed7804479f1a12d712a93851a71ac (diff)
Support des socles.
Diffstat (limited to 'n/asserv/src/taz.c')
-rw-r--r--n/asserv/src/taz.c122
1 files changed, 119 insertions, 3 deletions
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: