From 74550d3214bca333dcbcd73b75a1e7b535c7e1f0 Mon Sep 17 00:00:00 2001 From: schodet Date: Tue, 3 May 2005 01:26:58 +0000 Subject: Ajout d'une vitesse max l et a. Ajout des coefs l et r. Ajout de goto_counter. Ajout des fonctions de goto setup. --- n/asserv/src/eeprom.c | 26 +++++++---- n/asserv/src/goto.c | 122 +++++++++++++++++++++++++++++++++++++++++++++----- n/asserv/src/main.c | 79 ++++++++++++++++---------------- n/asserv/src/speed.c | 105 ++++++++++++++++++++++++++++++++----------- n/asserv/src/taz.c | 99 +++++++++------------------------------- 5 files changed, 267 insertions(+), 164 deletions(-) (limited to 'n') 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 -#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 #include +/** 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; } -- cgit v1.2.3