summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/main.c11
-rw-r--r--n/asserv/src/taz.c264
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: