summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/goto.c6
-rw-r--r--n/asserv/src/main.c12
-rw-r--r--n/asserv/src/speed.c2
-rw-r--r--n/asserv/src/taz.c291
4 files changed, 280 insertions, 31 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 553a4f4..64d1eac 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -88,7 +88,7 @@ goto_angular_mode (void)
angle_diff <<= 8;
angle_diff >>= 8;
/* Small angles. */
- if (0x10000 > angle_diff && angle_diff < -0x10000)
+ if (0x10000L > angle_diff && angle_diff > -0x10000L)
{
goto_finish = 1;
speed_left = 0;
@@ -182,12 +182,12 @@ goto_ftw_mode (void)
speed_right = 0;
speed_right_aim = 0;
}
- if (!(PINA & (_BV (0) || _BV (7))))
+ if (!(PINA & (_BV (0) | _BV (7))))
goto_finish = 1;
}
/** Update the speed according to the desired destination. */
-static inline void
+static void
goto_update (void)
{
switch (goto_mode)
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 1715d5f..1513876 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -38,6 +38,9 @@
#include "postrack.c"
#include "goto.c"
+/** Use Taz. */
+uint8_t motor_taz;
+
/** Motor mode :
* 0 - pwm setup.
* 1 - speed control.
@@ -77,6 +80,8 @@ uint16_t motor_sharps[3];
uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3,
motor_timer_4;
+#include "taz.c"
+
/* +AutoDec */
/* Main loop. */
@@ -97,6 +102,7 @@ main (void)
postrack_init ();
rs232_init ();
twi_master_init ();
+ taz_init ();
proto_send0 ('z');
sei ();
main_loop ();
@@ -117,6 +123,9 @@ main_loop (void)
/* Position tracking. */
postrack_update ();
motor_timer_3 = timer_read ();
+ /* Taz! */
+ if (motor_taz)
+ taz_update ();
/* Position control. */
if (motor_mode >= 2)
{
@@ -206,6 +215,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
reset ();
break;
/* Commands. */
+ case c ('t', 0):
+ motor_taz = 1;
+ break;
case c ('l', 2):
/* Linear move.
* - w: distance (negative to go backward). */
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 84f6f39..8050b29 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -30,7 +30,7 @@ int8_t speed_left_aim, speed_right_aim;
/** Max speed. */
int8_t speed_max = 0x20;
/** Acceleration value. */
-uint8_t speed_acc = 4;
+uint8_t speed_acc = 8;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
/** Integral term. */
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);
}