summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2005-05-03 01:26:58 +0000
committerschodet2005-05-03 01:26:58 +0000
commit74550d3214bca333dcbcd73b75a1e7b535c7e1f0 (patch)
tree58a9a4a54db4cc884bc062050eb93a62d8f77c4e
parentdaea50b1df19aea257018e4725463f33ce09b937 (diff)
Ajout d'une vitesse max l et a.
Ajout des coefs l et r. Ajout de goto_counter. Ajout des fonctions de goto setup.
-rw-r--r--n/asserv/src/eeprom.c26
-rw-r--r--n/asserv/src/goto.c122
-rw-r--r--n/asserv/src/main.c79
-rw-r--r--n/asserv/src/speed.c105
-rw-r--r--n/asserv/src/taz.c99
5 files changed, 267 insertions, 164 deletions
diff --git a/n/asserv/src/eeprom.c b/n/asserv/src/eeprom.c
index 4cd3a10..b454fdd 100644
--- a/n/asserv/src/eeprom.c
+++ b/n/asserv/src/eeprom.c
@@ -23,7 +23,7 @@
* }}} */
#include <avr/eeprom.h>
-#define EEPROM_KEY 0xa7
+#define EEPROM_KEY 0xa9
/* +AutoDec */
/* -AutoDec */
@@ -37,13 +37,17 @@ eeprom_read_params (void)
if (eeprom_read_byte (p8++) != EEPROM_KEY)
return;
speed_acc_cpt = speed_acc = eeprom_read_byte (p8++);
- speed_max = eeprom_read_byte (p8++);
+ speed_max_l = eeprom_read_byte (p8++);
+ speed_max_a = eeprom_read_byte (p8++);
speed_ds = eeprom_read_byte (p8++);
pwm_dir = eeprom_read_byte (p8++);
p16 = (uint16_t *) p8;
- speed_kp = eeprom_read_word (p16++);
- speed_ki = eeprom_read_word (p16++);
- speed_kd = eeprom_read_word (p16++);
+ speed_left_kp = eeprom_read_word (p16++);
+ speed_right_kp = eeprom_read_word (p16++);
+ speed_left_ki = eeprom_read_word (p16++);
+ speed_right_ki = eeprom_read_word (p16++);
+ speed_left_kd = eeprom_read_word (p16++);
+ speed_right_kd = eeprom_read_word (p16++);
speed_e_sat = eeprom_read_word (p16++);
speed_int_sat = eeprom_read_word (p16++);
postrack_set_footing (eeprom_read_word (p16++));
@@ -57,13 +61,17 @@ eeprom_write_params (void)
uint16_t *p16;
eeprom_write_byte (p8++, EEPROM_KEY);
eeprom_write_byte (p8++, speed_acc);
- eeprom_write_byte (p8++, speed_max);
+ eeprom_write_byte (p8++, speed_max_l);
+ eeprom_write_byte (p8++, speed_max_a);
eeprom_write_byte (p8++, speed_ds);
eeprom_write_byte (p8++, pwm_dir);
p16 = (uint16_t *) p8;
- eeprom_write_word (p16++, speed_kp);
- eeprom_write_word (p16++, speed_ki);
- eeprom_write_word (p16++, speed_kd);
+ eeprom_write_word (p16++, speed_left_kp);
+ eeprom_write_word (p16++, speed_right_kp);
+ eeprom_write_word (p16++, speed_left_ki);
+ eeprom_write_word (p16++, speed_right_ki);
+ eeprom_write_word (p16++, speed_left_kd);
+ eeprom_write_word (p16++, speed_right_kd);
eeprom_write_word (p16++, speed_e_sat);
eeprom_write_word (p16++, speed_int_sat);
eeprom_write_word (p16++, postrack_footing);
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index d35f9fa..fdc5953 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -27,6 +27,7 @@
* 0: linear move.
* 1: angular move.
* 2: position move.
+ * 4: counter move.
* 10: we fuck the wall.
*/
uint8_t goto_mode;
@@ -34,7 +35,9 @@ uint8_t goto_mode;
uint16_t goto_d;
/** Sign of movement (0: forward, 1 backward). */
uint8_t goto_sign;
-/** Destination position, f24.8. */
+/** Counter aim. */
+int16_t goto_counter_left, goto_counter_right;
+/** Destination/start position, f24.8. */
int32_t goto_x, goto_y;
/** Destination angle, f0.8. */
int8_t goto_a;
@@ -51,11 +54,11 @@ uint8_t goto_finish;
/* -AutoDec */
/** Linear control mode. */
-static inline void
+static void
goto_linear_mode (void)
{
uint16_t d;
- uint32_t dx, dy;
+ int32_t dx, dy;
/* Compute distance from the start point.
* WARNING: this could overflow as dsp_hypot accept a maximum value of
* +/- 65535. */
@@ -80,8 +83,22 @@ goto_linear_mode (void)
}
}
-/** Angular control mode. */
+/** Setup linear mode. */
static inline void
+goto_linear (int16_t d)
+{
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = d >> 15;
+ goto_d = d;
+ if (goto_sign) goto_d = -goto_d;
+ goto_x = postrack_x;
+ goto_y = postrack_y;
+ goto_finish = 0;
+}
+
+/** Angular control mode. */
+static void
goto_angular_mode (void)
{
int32_t angle_diff;
@@ -90,7 +107,7 @@ goto_angular_mode (void)
angle_diff <<= 8;
angle_diff >>= 8;
/* Small angles. */
- if (0x10000L > angle_diff && angle_diff > -0x10000L)
+ if (0x1000L > angle_diff && angle_diff > -0x1000L)
{
speed_left = 0;
speed_right = 0;
@@ -107,11 +124,22 @@ goto_angular_mode (void)
}
}
-/** Position control mode. */
+/** Setup angular mode. */
static inline void
+goto_angular (int8_t a)
+{
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = a;
+ goto_finish = 0;
+}
+
+/** Position control mode. */
+static void
goto_position_mode (void)
{
int32_t c, s, arc;
+ int32_t dla, daa;
goto_dx = goto_x - postrack_x; /* f24.8 */
goto_dy = goto_y - postrack_y;
if (goto_dx < goto_eps && goto_dx > -goto_eps
@@ -130,33 +158,63 @@ goto_position_mode (void)
s = dsp_sin_f824 (postrack_a);
goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s);
goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s);
+ if (goto_dl > 0)
+ dla = goto_dl;
+ else
+ dla = -goto_dl;
+ if (goto_da > 0)
+ daa = goto_da;
+ else
+ daa = -goto_da;
/* If very big angle (> 83 °), rotate. */
- if (goto_da > goto_dl * 8)
+ if (daa > dla * 8)
{
/* Compute arc. */
arc = postrack_footing_2pi / 4;
speed_distance (0, goto_da > 0 ? arc : -arc);
}
/* If big angle (> 1.7°), rotate. */
- else if (goto_da * 32 > goto_dl)
+ else if (daa * 32 > dla)
{
/* Compute arc. This is a rough aproximation. */
arc = goto_da / (goto_dl >> 8) * (postrack_footing / 2);
speed_distance (0, arc);
}
/* Is small angle (< 0.44°), strait ahead. */
- else if (goto_da * 128 < goto_dl)
+ else if (daa * 128 < dla)
{
speed_distance (goto_dl, 0);
}
- /* Else, curve. TODO. */
+ /* Else, curve. */
else
{
speed_distance (goto_dl, 0);
+ if ((goto_da >= 0 && goto_dl >= 0)
+ || (goto_da < 0 && goto_dl < 0))
+ {
+ speed_left_aim--;
+ speed_right_aim++;
+ }
+ else
+ {
+ speed_left_aim++;
+ speed_right_aim--;
+ }
}
}
}
+/** Setup position mode. */
+static inline void
+goto_position (int32_t x, int32_t y)
+{
+ motor_mode = 2;
+ goto_mode = 2;
+ goto_x = x;
+ goto_y = y;
+ goto_finish = 0;
+}
+
/** Position control mode, `exact' method. */
static inline void
goto_position_exact_mode (void)
@@ -184,8 +242,37 @@ goto_position_exact_mode (void)
}
}
-/** We fuck the wall mode. */
+/** Counter control mode. */
+static void
+goto_counter_mode (void)
+{
+ int16_t dl, dr;
+ 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)
+ {
+ goto_finish = 1;
+ }
+ dl248 = dl;
+ dr248 = dr;
+ speed_distance_lr (dl248 << 8, dr248 << 8);
+}
+
+/** Setup counter mode. */
static inline void
+goto_counter (int16_t l, int16_t r)
+{
+ motor_mode = 2;
+ goto_mode = 4;
+ goto_counter_left = speed_left_counter + l;
+ goto_counter_right = speed_right_counter + r;
+ goto_finish = 0;
+}
+
+/** We fuck the wall mode. */
+static void
goto_ftw_mode (void)
{
/* Change speed. */
@@ -211,6 +298,16 @@ goto_ftw_mode (void)
goto_finish = 1;
}
+/** Setup ftw mode. */
+static inline void
+goto_ftw (int8_t s)
+{
+ motor_mode = 2;
+ goto_mode = 10;
+ goto_s = s;
+ goto_finish = 0;
+}
+
/** Update the speed according to the desired destination. */
static void
goto_update (void)
@@ -226,6 +323,9 @@ goto_update (void)
case 2:
goto_position_mode ();
break;
+ case 4:
+ goto_counter_mode ();
+ break;
case 10:
goto_ftw_mode ();
break;
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index e6c2303..ef607b3 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -32,6 +32,12 @@
#include <avr/io.h>
#include <avr/signal.h>
+/** Motor mode :
+ * 0 - pwm setup.
+ * 1 - speed control.
+ * 2 - position control. */
+int8_t motor_mode;
+
#include "pwm.c"
#include "timer.c"
#include "counter.c"
@@ -43,12 +49,6 @@
/** Use Taz. */
uint8_t motor_taz;
-/** Motor mode :
- * 0 - pwm setup.
- * 1 - speed control.
- * 2 - position control. */
-int8_t motor_mode;
-
/** Main loop counter. */
uint8_t motor_loop_cpt;
@@ -97,6 +97,7 @@ int
main (void)
{
DDRD = 0x60;
+ PORTA = _BV (0) | _BV (7);
pwm_init ();
timer_init ();
counter_init ();
@@ -140,11 +141,7 @@ main_loop (void)
motor_timer_2 = timer_read ();
/* Speed control. */
if (motor_mode >= 1)
- {
speed_update ();
- speed_compute_left_pwm ();
- speed_compute_right_pwm ();
- }
motor_timer_1 = timer_read ();
/* Pwm setup. */
pwm_update ();
@@ -229,40 +226,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('l', 2):
/* Linear move.
* - w: distance (negative to go backward). */
- motor_mode = 2;
- goto_mode = 0;
- goto_sign = args[0] >> 7;
- goto_d = v8_to_v16 (args[0], args[1]);
- if (goto_sign) goto_d = -goto_d;
- goto_x = postrack_x;
- goto_y = postrack_y;
- goto_finish = 0;
+ goto_linear (v8_to_v16 (args[0], args[1]));
break;
case c ('a', 1):
/* Angular move.
* - b: absolute angle. */
- motor_mode = 2;
- goto_mode = 1;
- goto_a = args[0];
- goto_finish = 0;
+ goto_angular (args[0]);
break;
case c ('g', 8):
/* Go to position move.
* - d: f24.8, x position.
* - d: f24.8, y position. */
- motor_mode = 2;
- goto_mode = 2;
- goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]);
- goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]);
- goto_finish = 0;
+ goto_position (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]));
+ break;
+ case c ('p', 4):
+ /* Counter mode. */
+ goto_counter (v8_to_v16 (args[0], args[1]),
+ v8_to_v16 (args[2], args[3]));
break;
case c ('f', 1):
/* Fuck the wall move.
* - b: speed. */
- motor_mode = 2;
- goto_mode = 10;
- goto_s = args[0];
- goto_finish = 0;
+ goto_ftw (args[0]);
break;
case c ('F', 1):
/* Setup finish flag.
@@ -365,13 +351,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
goto_eps = v8_to_v32 (args[1], args[2], args[3], args[4]);
break;
case c ('p', 3):
- speed_kp = v8_to_v16 (args[1], args[2]);
+ speed_left_kp = speed_right_kp = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('p', 5):
+ speed_left_kp = v8_to_v16 (args[1], args[2]);
+ speed_right_kp = v8_to_v16 (args[3], args[4]);
break;
case c ('i', 3):
- speed_ki = v8_to_v16 (args[1], args[2]);
+ speed_left_ki = speed_right_ki = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('i', 5):
+ speed_left_ki = v8_to_v16 (args[1], args[2]);
+ speed_right_ki = v8_to_v16 (args[3], args[4]);
break;
case c ('d', 3):
- speed_kd = v8_to_v16 (args[1], args[2]);
+ speed_left_kd = speed_right_kd = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('d', 5):
+ speed_left_kd = v8_to_v16 (args[1], args[2]);
+ speed_right_kd = v8_to_v16 (args[3], args[4]);
break;
case c ('E', 3):
speed_e_sat = v8_to_v16 (args[1], args[2]);
@@ -386,9 +384,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set acceleration. */
speed_acc_cpt = speed_acc = args[1];
break;
- case c ('m', 2):
+ case c ('m', 3):
/* Set maximum speed for automatic movements. */
- speed_max = args[1];
+ speed_max_l = args[1];
+ speed_max_a = args[2];
break;
case c ('w', 3):
/* Set PWM direction.
@@ -411,10 +410,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1b ('E', EEPROM_KEY);
proto_send1w ('f', postrack_footing);
proto_send1b ('a', speed_acc);
- proto_send1b ('m', speed_max);
- proto_send1w ('p', speed_kp);
- proto_send1w ('i', speed_ki);
- proto_send1w ('d', speed_kd);
+ proto_send2b ('m', speed_max_l, speed_max_a);
+ proto_send2w ('p', speed_left_kp, speed_right_kp);
+ proto_send2w ('i', speed_left_ki, speed_right_ki);
+ proto_send2w ('d', speed_left_kd, speed_right_kd);
proto_send1w ('E', speed_e_sat);
proto_send1w ('I', speed_int_sat);
proto_send1b ('s', speed_ds);
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 06b3abc..dd326a6 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -28,9 +28,9 @@ int8_t speed_left, speed_right;
/** Wanted speed. */
int8_t speed_left_aim, speed_right_aim;
/** Max speed for automatic movements. */
-int8_t speed_max = 0x10;
+int8_t speed_max_l = 0x08, speed_max_a = 0x05;
/** Acceleration value. */
-uint8_t speed_acc = 0x40;
+uint8_t speed_acc = 0x60;
/** Acceleration counter, speed gets updated when it reachs 0. */
uint8_t speed_acc_cpt;
/** Error saturation. */
@@ -38,11 +38,11 @@ int16_t speed_e_sat = 1023;
/** Integral saturation. */
int16_t speed_int_sat = 8191;
/** P coeficients. 4.8 fixed point format. */
-uint16_t speed_kp = 0x0fff;
+uint16_t speed_left_kp = 0x0fff, speed_right_kp = 0x0fff;
/** I coeficients. 3.8 fixed point format. */
-uint16_t speed_ki = 0x0008;
+uint16_t speed_left_ki = 0x0008, speed_right_ki = 0x0008;
/** D coeficients. 3.8 fixed point format. */
-uint16_t speed_kd = 0x0fff;
+uint16_t speed_left_kd = 0x0fff, speed_right_kd = 0x0fff;
/** D sample period. */
uint8_t speed_ds = 2;
/** Integral term. */
@@ -83,6 +83,16 @@ speed_compute_right_pwm (void);
static inline void
speed_restart (void);
+/** Set maximum speed in order to be able to brake before a distance and
+ * angle, inputs are f24.8 */
+static inline void
+speed_distance (int32_t dist, int32_t arc);
+
+/** Set maximum left & right speeds in order to be able to brake. Inputs are
+ * f24.8 */
+static inline void
+speed_distance_lr (int32_t left, int32_t right);
+
/* -AutoDec */
/** Initialise speed parameters. */
@@ -95,6 +105,7 @@ speed_init (void)
static inline void
speed_update (void)
{
+ /* Accelerate. */
if (speed_acc)
{
speed_acc_cpt--;
@@ -127,6 +138,13 @@ speed_update (void)
speed_left_counter += speed_left;
speed_right_counter += speed_right;
}
+ /* Compute pwm. */
+ speed_compute_left_pwm ();
+ speed_compute_right_pwm ();
+ /* Update D history pointer. */
+ speed_d_hist_ptr++;
+ if (speed_d_hist_ptr >= speed_ds)
+ speed_d_hist_ptr = 0;
}
/** Compute new pwm value for left motor. */
@@ -135,29 +153,26 @@ speed_compute_left_pwm (void)
{
int16_t e, diff, pwm;
/* Get and saturate error. */
- e = speed_left_counter - counter_left; /* 11b */
+ e = speed_left_counter - counter_left;
if (e > speed_e_sat)
e = speed_e_sat;
else if (e < -speed_e_sat)
e = -speed_e_sat;
/* Integral update. */
- speed_left_int += e; /* 12b = 11b + 10b */
- if (speed_left_int > speed_int_sat) /* 11b */
+ speed_left_int += e;
+ if (speed_left_int > speed_int_sat)
speed_left_int = speed_int_sat;
else if (speed_left_int < -speed_int_sat)
speed_left_int = -speed_int_sat;
/* Differential value. */
- diff = e - speed_left_e_old; /* 11b */
+ diff = e - speed_left_e_old;
speed_left_d_hist_sum += diff;
speed_left_d_hist_sum -= speed_left_d_hist[speed_d_hist_ptr];
speed_left_d_hist[speed_d_hist_ptr] = diff;
- speed_d_hist_ptr++;
- if (speed_d_hist_ptr >= speed_ds)
- speed_d_hist_ptr = 0;
/* Compute PI. */ /* 16b = 15b + 14b + 14b */
- pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b */
- + dsp_mul_i16f88 (speed_left_int, speed_ki) /* 14b */
- + dsp_mul_i16f88 (speed_left_d_hist_sum, speed_kd); /* 14b */
+ pwm = dsp_mul_i16f88 (e, speed_left_kp)
+ + dsp_mul_i16f88 (speed_left_int, speed_left_ki)
+ + dsp_mul_i16f88 (speed_left_d_hist_sum, speed_left_kd);
/* Save result. */
speed_left_e_old = e;
pwm_left = pwm;
@@ -169,26 +184,26 @@ speed_compute_right_pwm (void)
{
int16_t e, diff, pwm;
/* Get and saturate error. */
- e = speed_right_counter - counter_right; /* 11b */
+ e = speed_right_counter - counter_right;
if (e > speed_e_sat)
e = speed_e_sat;
else if (e < -speed_e_sat)
e = -speed_e_sat;
/* Integral update. */
- speed_right_int += e; /* 12b = 11b + 10b */
- if (speed_right_int > speed_int_sat) /* 11b */
+ speed_right_int += e;
+ if (speed_right_int > speed_int_sat)
speed_right_int = speed_int_sat;
else if (speed_right_int < -speed_int_sat)
speed_right_int = -speed_int_sat;
/* Differential value. */
- diff = e - speed_right_e_old; /* 11b */
+ diff = e - speed_right_e_old;
speed_right_d_hist_sum += diff;
speed_right_d_hist_sum -= speed_right_d_hist[speed_d_hist_ptr];
speed_right_d_hist[speed_d_hist_ptr] = diff;
/* Compute PI. */ /* 16b = 15b + 14b + 14b */
- pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b */
- + dsp_mul_i16f88 (speed_right_int, speed_ki) /* 14b */
- + dsp_mul_i16f88 (speed_right_d_hist_sum, speed_kd); /* 14b */
+ pwm = dsp_mul_i16f88 (e, speed_right_kp)
+ + dsp_mul_i16f88 (speed_right_int, speed_right_ki)
+ + dsp_mul_i16f88 (speed_right_d_hist_sum, speed_right_kd);
/* Save result. */
speed_right_e_old = e;
pwm_right = pwm;
@@ -217,7 +232,7 @@ speed_restart (void)
}
}
-/** Set maximum speed in order to be able to break before a distance and
+/** Set maximum speed in order to be able to brake before a distance and
* angle, inputs are f24.8 */
static inline void
speed_distance (int32_t dist, int32_t arc)
@@ -239,10 +254,10 @@ speed_distance (int32_t dist, int32_t arc)
va248 = dsp_sqrt_f248 (arc * 2 / speed_acc);
va = (va248 >> 8) & 0xff;
/* Saturate. */
- if (vl248 & 0xffff0000 || vl > speed_max)
- vl = speed_max;
- if (va248 & 0xffff0000 || va > speed_max)
- va = speed_max;
+ if (vl248 & 0xffff0000 || vl > speed_max_l)
+ vl = speed_max_l;
+ if (va248 & 0xffff0000 || va > speed_max_a)
+ va = speed_max_a;
/* Get sign back. */
if (vls) vl = -vl;
if (vas) va = -va;
@@ -250,3 +265,39 @@ speed_distance (int32_t dist, int32_t arc)
speed_right_aim = vl + va;
}
+/** Set maximum left & right speeds in order to be able to brake. Inputs are
+ * f24.8 */
+static inline void
+speed_distance_lr (int32_t left, int32_t right)
+{
+ uint8_t vls, vrs;
+ uint32_t vl248, vr248;
+ int8_t vl, vr;
+ uint8_t max;
+ /* Drop sign. */
+ vls = left < 0;
+ if (vls) left = -left;
+ vrs = right < 0;
+ if (vrs) right = -right;
+ /* Compute speed.
+ * v = sqrt (2 * a * d), a = 1/speed_acc */
+ vl248 = dsp_sqrt_f248 (left * 2 / speed_acc);
+ vl = (vl248 >> 8) & 0xff;
+ vr248 = dsp_sqrt_f248 (right * 2 / speed_acc);
+ vr = (vr248 >> 8) & 0xff;
+ /* Saturate. */
+ if (vls != vrs)
+ max = speed_max_l;
+ else
+ max = speed_max_a;
+ if (vl248 & 0xffff0000 || vl > max)
+ vl = max;
+ if (vr248 & 0xffff0000 || vr > max)
+ vr = max;
+ /* Get sign back. */
+ if (vls) vl = -vl;
+ if (vrs) vr = -vr;
+ speed_left_aim = vl;
+ speed_right_aim = vr;
+}
+
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index 60c8330..913baa6 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -106,10 +106,7 @@ taz_state_0 (void)
taz_substate = 2;
/* FTW. */
speed_restart ();
- motor_mode = 2;
- goto_mode = 10;
- goto_s = -10;
- goto_finish = 0;
+ goto_ftw (-3);
}
break;
case 2:
@@ -121,13 +118,7 @@ taz_state_0 (void)
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_16 - taz_rear_16;
- goto_x = postrack_x;
- goto_y = postrack_y;
- goto_finish = 0;
+ goto_linear (taz_start_y_16 - taz_rear_16);
}
break;
case 3:
@@ -135,10 +126,7 @@ taz_state_0 (void)
{
taz_substate = 4;
/* Direction l'axe des x. */
- motor_mode = 2;
- goto_mode = 1;
- goto_a = 0;
- goto_finish = 0;
+ goto_angular (0);
}
break;
case 4:
@@ -146,10 +134,7 @@ taz_state_0 (void)
{
taz_substate = 5;
/* FTW. */
- motor_mode = 2;
- goto_mode = 10;
- goto_s = -10;
- goto_finish = 0;
+ goto_ftw (-3);
}
break;
case 5:
@@ -188,13 +173,7 @@ taz_state_1 (void)
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;
+ goto_linear (taz_before_bridge_16 - taz_rear_16);
break;
case 1:
if (goto_finish)
@@ -218,13 +197,13 @@ taz_state_1 (void)
&& motor_sharps[2] > 0x117)
taz_pont = 0;
else if (motor_sharps[0] < 0x117 && motor_sharps[1] > 0x117
- && motor_sharps[2] > 0x117)
+ && motor_sharps[2] > 0x117)
taz_pont = 1;
else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117
- && motor_sharps[2] > 0x117)
+ && motor_sharps[2] > 0x117)
taz_pont = 2;
else if (motor_sharps[0] < 0x117 && motor_sharps[1] < 0x117
- && motor_sharps[2] < 0x117)
+ && motor_sharps[2] < 0x117)
taz_pont = 3;
else
{
@@ -233,16 +212,14 @@ taz_state_1 (void)
}
if (taz_pont == 0)
{
+ /* Traversée directe. */
taz_substate = 10;
}
else
{
- /* Direction l'axe des y. */
taz_substate = 6;
- motor_mode = 2;
- goto_mode = 1;
- goto_a = 0xc0;
- goto_finish = 0;
+ /* Direction l'axe des y. */
+ goto_angular (0xc0);
}
}
break;
@@ -251,10 +228,7 @@ taz_state_1 (void)
{
taz_substate = 5;
/* FTW. */
- motor_mode = 2;
- goto_mode = 10;
- goto_s = -10;
- goto_finish = 0;
+ goto_ftw (-3);
}
break;
case 5:
@@ -272,14 +246,9 @@ taz_state_1 (void)
{
taz_substate = 7;
/* 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_pont * taz_brige_interval_16 +
- (postrack_y >> 8);
- goto_x = postrack_x;
- goto_y = postrack_y;
- goto_finish = 0;
+ goto_linear (taz_start_y_16
+ + taz_pont * taz_brige_interval_16
+ + (postrack_y >> 8));
}
break;
case 7:
@@ -287,10 +256,7 @@ taz_state_1 (void)
{
taz_substate = 8;
/* Direction l'axe des x. */
- motor_mode = 2;
- goto_mode = 1;
- goto_a = 0;
- goto_finish = 0;
+ goto_angular (0);
}
break;
case 8:
@@ -342,12 +308,9 @@ taz_state_1 (void)
{
taz_pont = 0;
}
- /* Direction l'axe des y. */
taz_substate = 4;
- motor_mode = 2;
- goto_mode = 1;
- goto_a = 0xc0;
- goto_finish = 0;
+ /* Direction l'axe des y. */
+ goto_angular (0xc0);
}
break;
case 10:
@@ -355,13 +318,7 @@ taz_state_1 (void)
{
taz_substate = 11;
/* 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;
+ goto_linear (taz_after_bridge_16[taz_pont] - (postrack_x >> 8));
}
break;
case 11:
@@ -369,10 +326,7 @@ taz_state_1 (void)
{
taz_substate = 12;
/* Tourne vers les quilles. */
- motor_mode = 2;
- goto_mode = 1;
- goto_a = taz_ang[taz_pont];
- goto_finish = 0;
+ goto_angular (taz_ang[taz_pont]);
}
break;
case 12:
@@ -381,13 +335,7 @@ taz_state_1 (void)
taz_substate = 13;
/* 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;
+ goto_linear (taz_ang_dist_16[taz_pont]);
}
break;
case 13:
@@ -395,10 +343,7 @@ taz_state_1 (void)
{
taz_substate = 14;
/* Tourne vers les quilles. */
- motor_mode = 2;
- goto_mode = 1;
- goto_a = 0;
- goto_finish = 0;
+ goto_angular (0);
}
break;
}