summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--n/asserv/src/goto.c4
-rw-r--r--n/asserv/src/main.c3
-rw-r--r--n/asserv/src/taz.c55
3 files changed, 48 insertions, 14 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 07ab840..1d8dffb 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -254,8 +254,8 @@ goto_counter_mode (void)
int32_t dl248, dr248;
dl = goto_counter_left - counter_left;
dr = goto_counter_right - counter_right;
- if (dl < 3 && dl > -3
- && dr < 3 && dr > -3)
+ if (dl < 20 && dl > -20
+ && dr < 20 && dr > -20)
{
if (!goto_finish)
goto_finish = 1;
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 0118540..b944f8a 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -237,6 +237,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
taz_substate = args[1];
motor_taz = args[2];
break;
+ case c ('T', 0):
+ proto_send1d ('T', taz_timer);
+ 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]);
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index aa7b9c3..533f634 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -27,7 +27,7 @@ uint8_t taz_state;
uint8_t taz_substate;
/** FSM tempo. */
uint16_t taz_tempo;
-static const uint16_t taz_1s = 900;
+#define taz_1s 900
/** FSM max state & substate. */
uint8_t taz_max_state = 0xff;
uint8_t taz_max_substate;
@@ -40,6 +40,9 @@ uint16_t taz_quilles_min = 0x160;
/** Min pour un socle vu de haut. */
uint16_t taz_socle_min = 0x171;
+/** Anti blocage. */
+uint8_t taz_bloc = 0;
+
/** Positions. */
/********************************
* *
@@ -87,7 +90,7 @@ static const uint16_t taz_ang_dist_16[4] =
/** Bridge position. */
uint8_t taz_pont;
/** Timer. */
-uint32_t taz_timer;
+uint32_t taz_timer = 90L * taz_1s;
/* +AutoDec */
/* -AutoDec */
@@ -170,7 +173,6 @@ taz_state_0 (void)
{
taz_state = 1;
taz_substate = 0;
- taz_timer = 0;
}
break;
}
@@ -604,12 +606,19 @@ taz_state_2 (void)
if (taz_dir_socle ())
{
taz_substate = 28;
- goto_linear (600 * taz_scale);
+ goto_linear (450 * taz_scale);
}
break;
case 28:
if (goto_finish)
{
+ taz_substate = 29;
+ goto_angular (0x60);
+ }
+ break;
+ case 29:
+ if (goto_finish)
+ {
uint8_t a = v32_to_v8 (postrack_a, 2) + 0xf0;
r = taz_find_quilles (a, 0);
if (r == 1)
@@ -663,16 +672,38 @@ taz_update (void)
old_state = taz_state;
old_substate = taz_substate;
/* Game timer. */
- taz_timer++;
- if (taz_state && taz_timer > 4 * 90 * taz_1s)
+ if (taz_state)
+ {
+ taz_timer--;
+ if (!taz_timer)
+ {
+ taz_state = 0xff;
+ taz_substate = 0xff;
+ speed_restart ();
+ motor_mode = 0;
+ pwm_left = 0;
+ pwm_right = 0;
+ proto_send2b ('t', 0xff, 0xff);
+ }
+ }
+ /* Blocages. */
+ if (taz_bloc)
+ {
+ if (!goto_finish)
+ {
+ return;
+ }
+ proto_send1b ('b', 0);
+ taz_bloc = 0;
+ }
+ else if ((speed_left_e_old == speed_e_sat || speed_left_e_old == -speed_e_sat)
+ && (speed_right_e_old == speed_e_sat || speed_right_e_old == -speed_e_sat))
{
- taz_state = 0xff;
- taz_substate = 0;
speed_restart ();
- motor_mode = 0;
- pwm_left = 0;
- pwm_right = 0;
- proto_send2b ('t', 0xff, 0xff);
+ goto_counter (-70 * taz_scale, -100 * taz_scale);
+ taz_bloc = 1;
+ proto_send1b ('b', 1);
+ return;
}
/* Max state. */
if (taz_state > taz_max_state