summaryrefslogtreecommitdiff
path: root/n/asserv/src/taz.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/taz.c')
-rw-r--r--n/asserv/src/taz.c291
1 files changed, 264 insertions, 27 deletions
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index 61187b8..c165e33 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -25,20 +25,45 @@
/** FSM state. */
uint8_t taz_state;
uint8_t taz_substate;
+uint8_t taz_tempo;
/** Positions. */
-#define taz_scale 0.05026548245743669181
+//#define taz_scale (1.0/0.05026548245743669181)
+#define taz_scale (1.0/.05090591755319148936)
static const uint16_t taz_rear_16 = taz_scale * 270 / 2;
-static const uint32_t taz_rear_32 = taz_rear_16 * 256;
+static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256;
static const uint16_t taz_front_16 = taz_scale * 270 / 2;
-static const uint32_t taz_front_32 = taz_front_16 * 256;
+static const uint32_t taz_front_32 = taz_scale * 270 / 2 * 256;
static const uint16_t taz_side_16 = taz_scale * 340 / 2;
-static const uint32_t taz_side_32 = taz_side_16 * 256;
+static const uint32_t taz_side_32 = taz_scale * 340 / 2 * 256;
-static const uint16_t taz_start_y_16 = taz_scale * 450 / 2;
+static const uint16_t taz_start_y_16 = taz_scale * (450 - 50) / 2;
-static const uint16_t taz_before_bridge_16 = taz_scale * 1050;
-static const uint16_t taz_after_bridge_16 = taz_scale * (1500 + 22 + 600 + 22
- + 300);
+static const uint16_t taz_before_bridge_16 = taz_scale * 1200;
+static const uint16_t taz_brige_interval_16 = taz_scale * 140;
+#define OR2 (1500 + 22 + 600 + 22)
+static const uint16_t taz_after_bridge_16[4] =
+{
+ taz_scale * (OR2),
+ taz_scale * (OR2 + 150),
+ taz_scale * (OR2 + 150),
+ taz_scale * (OR2 + 150)
+};
+static const uint8_t taz_ang[4] =
+{
+ -18,
+ -10,
+ 13,
+ -8
+};
+static const uint16_t taz_ang_dist_16[4] =
+{
+ taz_scale * (496),
+ taz_scale * (290),
+ taz_scale * (316),
+ taz_scale * (306)
+};
+
+uint8_t taz_pont;
/* +AutoDec */
/* -AutoDec */
@@ -51,30 +76,28 @@ static const uint16_t taz_after_bridge_16 = taz_scale * (1500 + 22 + 600 + 22
static inline void
taz_init (void)
{
- taz_state = 0;
- taz_substate = 0;
}
/** Mise en place avant le départ. */
-static inline void
+static void
taz_state_0 (void)
{
switch (taz_substate)
{
case 0:
/* Attend que l'on enfonce le jack. */
- if (PINA & _BV (6))
+ if (!(PINA & _BV (6)))
taz_substate = 1;
break;
case 1:
/* Attend que l'on enlève le jack. */
- if (!(PINA & _BV (6)))
+ if (PINA & _BV (6))
{
taz_substate = 2;
/* FTW. */
motor_mode = 2;
goto_mode = 10;
- goto_s = -0x10;
+ goto_s = -10;
goto_finish = 0;
}
break;
@@ -83,13 +106,14 @@ taz_state_0 (void)
{
taz_substate = 3;
/* Recalage. */
- goto_y = taz_rear_32;
- goto_a = 0x00400000;
+ speed_restart ();
+ postrack_y = -taz_rear_32;
+ postrack_a = 0x00c00000;
/* On avance juste qu'à l'y de départ. */
motor_mode = 2;
goto_mode = 0;
goto_sign = 0;
- goto_d = taz_start_y - taz_rear;
+ goto_d = taz_start_y_16 - taz_rear_16;
goto_x = postrack_x;
goto_y = postrack_y;
goto_finish = 0;
@@ -113,40 +137,243 @@ taz_state_0 (void)
/* FTW. */
motor_mode = 2;
goto_mode = 10;
- goto_s = -0x10;
+ goto_s = -10;
goto_finish = 0;
}
break;
case 5:
- if (goto_finish && (PINA & _BV (6)))
+ if (goto_finish)
{
taz_substate = 6;
/* Recalage. */
- goto_x = taz_rear_32;
- goto_a = 0;
+ speed_restart ();
+ postrack_x = taz_rear_32;
+ postrack_a = 0;
}
+ break;
case 6:
if (!(PINA & _BV (6)))
{
+ taz_substate = 7;
+ }
+ break;
+ case 7:
+ if (PINA & _BV (6))
+ {
taz_state = 1;
taz_substate = 0;
}
+ break;
}
}
/** Avancée vers le pont. */
-static inline void
+static void
taz_state_1 (void)
{
switch (taz_substate)
{
case 0:
+ taz_substate = 1;
+ /* On avance juste qu'au pont. */
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = 0;
+ goto_d = taz_before_bridge_16 - taz_rear_16;
+ goto_x = postrack_x;
+ goto_y = postrack_y;
+ goto_finish = 0;
+ break;
+ case 1:
+ if (goto_finish)
+ {
+ taz_substate = 2;
+ taz_tempo = 250;
+ }
+ break;
+ case 2:
+ taz_substate = 3;
+ /* Lance la capture. */
+ twi_master_read (0x02, (uint8_t *) motor_sharps, 6);
+ break;
+ 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. */
+ if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117
+ && motor_sharps[2] > 0x117)
+ taz_pont = 0;
+ else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117
+ && motor_sharps[2] > 0x117)
+ taz_pont = 1;
+ else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117
+ && motor_sharps[2] > 0x117)
+ taz_pont = 2;
+ else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117
+ && motor_sharps[2] < 0x117)
+ taz_pont = 3;
+ else
+ {
+ taz_substate = 1;
+ break;
+ }
+ /* Direction l'axe des y. */
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = 0xc0;
+ goto_finish = 0;
+ }
+ break;
+ case 4:
+ if (goto_finish)
+ {
+ taz_substate = 5;
+ /* FTW. */
+ motor_mode = 2;
+ goto_mode = 10;
+ goto_s = -10;
+ goto_finish = 0;
+ }
+ break;
+ case 5:
+ if (goto_finish)
+ {
+ taz_substate = 6;
+ /* Recalage. */
+ speed_restart ();
+ postrack_y = taz_rear_32;
+ postrack_a = 0x00c00000;
+ /* 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_x = postrack_x;
+ goto_y = postrack_y;
+ goto_finish = 0;
+ }
+ break;
+ case 6:
+ if (goto_finish)
+ {
+ taz_substate = 7;
+ /* Direction l'axe des x. */
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = 0;
+ goto_finish = 0;
+ }
+ break;
+ case 7:
+ if (goto_finish)
+ {
+ taz_substate = 8;
+ /* Reprend une mesure. */
+ twi_master_read (0x02, (uint8_t *) motor_sharps, 6);
+ }
+ break;
+ case 8:
+ 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;
+ break;
+ }
+ else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117
+ && motor_sharps[2] > 0x117 && taz_pont < 3)
+ {
+ taz_pont++;
+ }
+ else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117
+ && motor_sharps[2] > 0x117 && taz_pont < 2)
+ {
+ taz_pont += 2;
+ }
+ else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117
+ && motor_sharps[2] < 0x117 && taz_pont < 1)
+ {
+ taz_pont += 3;
+ }
+ else if (motor_sharps[0] > 0x117 && motor_sharps[1] < 0x117
+ && motor_sharps[2] < 0x117 && taz_pont > 1)
+ {
+ taz_pont -= 2;
+ }
+ else if (motor_sharps[0] > 0x117 && motor_sharps[1] > 0x117
+ && motor_sharps[2] < 0x117 && taz_pont > 0)
+ {
+ taz_pont--;
+ }
+ else
+ {
+ taz_pont = 0;
+ }
+ /* Direction l'axe des y. */
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = 0xc0;
+ goto_finish = 0;
+ taz_substate = 4;
+ }
+ break;
+ case 9:
+ if (goto_finish)
+ {
+ taz_substate = 10;
+ /* Tourne vers les quilles. */
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = taz_ang[taz_pont];
+ goto_finish = 0;
+ }
+ break;
+ case 10:
+ if (goto_finish)
+ {
+ taz_substate = 11;
+ /* Avance la petite distance, pour arriver sur la première quille. */
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = 0;
+ goto_d = taz_ang_dist_16[taz_pont];
+ 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;
+ goto_a = 0;
+ goto_finish = 0;
+ }
break;
}
}
/** Traversée du pont. */
-static inline void
+static void
taz_state_2 (void)
{
switch (taz_substate)
@@ -157,7 +384,7 @@ taz_state_2 (void)
}
/** . */
-static inline void
+static void
taz_state_3 (void)
{
switch (taz_substate)
@@ -168,7 +395,7 @@ taz_state_3 (void)
}
/** . */
-static inline void
+static void
taz_state_4 (void)
{
switch (taz_substate)
@@ -179,7 +406,7 @@ taz_state_4 (void)
}
/** . */
-static inline void
+static void
taz_state_5 (void)
{
switch (taz_substate)
@@ -190,9 +417,17 @@ taz_state_5 (void)
}
/** FSM. */
-static inline void
+static void
taz_update (void)
{
+ uint8_t old_state, old_substate;
+ old_state = taz_state;
+ old_substate = taz_substate;
+ if (taz_tempo)
+ {
+ taz_tempo--;
+ return;
+ }
switch (taz_state)
{
case 0:
@@ -214,4 +449,6 @@ taz_update (void)
taz_state_5 ();
break;
}
+ if (old_state != taz_state || old_substate != taz_substate)
+ proto_send2b ('t', taz_state, taz_substate);
}