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