From eb001ccd562cba753fc593608f80f3ee2bb971ab Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 30 Apr 2006 17:48:31 +0000 Subject: Ajout de l'idée géniale de Djerem pour l'acquitement. Changement des défauts pour les sens de rotation/codage. --- n/asserv/src/asserv/counter.avr.c | 4 ++-- n/asserv/src/asserv/eeprom.avr.c | 12 +++++++---- n/asserv/src/asserv/main.c | 42 +++++++++++++++++++++++---------------- n/asserv/src/asserv/pwm.avr.c | 2 +- n/asserv/src/asserv/speed.c | 20 ++++++++++--------- 5 files changed, 47 insertions(+), 33 deletions(-) diff --git a/n/asserv/src/asserv/counter.avr.c b/n/asserv/src/asserv/counter.avr.c index b0d14ee..c2b5878 100644 --- a/n/asserv/src/asserv/counter.avr.c +++ b/n/asserv/src/asserv/counter.avr.c @@ -24,9 +24,9 @@ * }}} */ /** Define to 1 to reverse the left counter. */ -#define COUNTER_REVERSE_LEFT 1 +#define COUNTER_REVERSE_LEFT 0 /** Define to 1 to reverse the right counter. */ -#define COUNTER_REVERSE_RIGHT 1 +#define COUNTER_REVERSE_RIGHT 0 /** Forward and reverse counter values. */ static uint8_t counter_left_frw, counter_left_rev, diff --git a/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c index 671e0e6..0ef63a0 100644 --- a/n/asserv/src/asserv/eeprom.avr.c +++ b/n/asserv/src/asserv/eeprom.avr.c @@ -24,7 +24,7 @@ * }}} */ #include -#define EEPROM_KEY 0x42 +#define EEPROM_KEY 0x43 #define EEPROM_START 256 /* +AutoDec */ @@ -34,10 +34,12 @@ static void eeprom_read_params (void) { - uint8_t *p8 = EEPROM_START; + uint8_t *p8 = (uint8_t *) EEPROM_START; uint16_t *p16; if (eeprom_read_byte (p8++) != EEPROM_KEY) return; + speed_theta_max = eeprom_read_byte (p8++); + speed_alpha_max = eeprom_read_byte (p8++); pwm_dir = eeprom_read_byte (p8++); p16 = (uint16_t *) p8; postrack_set_footing (eeprom_read_word (p16++)); @@ -57,9 +59,11 @@ eeprom_read_params (void) static void eeprom_write_params (void) { - uint8_t *p8 = EEPROM_START; + uint8_t *p8 = (uint8_t *) EEPROM_START; uint16_t *p16; eeprom_write_byte (p8++, EEPROM_KEY); + eeprom_write_byte (p8++, speed_theta_max); + eeprom_write_byte (p8++, speed_alpha_max); eeprom_write_byte (p8++, pwm_dir); p16 = (uint16_t *) p8; eeprom_write_word (p16++, postrack_footing); @@ -79,7 +83,7 @@ eeprom_write_params (void) static void eeprom_clear_params (void) { - uint8_t *p = EEPROM_START; + uint8_t *p = (uint8_t *) EEPROM_START; eeprom_write_byte (p, 0xff); } diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index c74f786..2a19877 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -54,6 +54,9 @@ * 3: trajectory control. */ int8_t main_mode; +/** Motor command sequence. */ +uint8_t main_sequence; + /** Report of counters. */ uint8_t main_stat_counter, main_stat_counter_cpt; @@ -77,8 +80,7 @@ uint8_t main_print_pin, main_print_pin_cpt; /** Record timer value at different stage of computing. Used for performance * analisys. */ -uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4, - main_timer_5; +uint8_t main_timer[6]; /* +AutoDec */ @@ -113,24 +115,24 @@ main (int argc, char **argv) static void main_loop (void) { - main_timer_5 = timer_read (); + main_timer[5] = timer_read (); timer_wait (); /* Counter update. */ counter_update (); - main_timer_0 = timer_read (); + main_timer[0] = timer_read (); /* Postion control. */ if (main_mode >= 1) pos_update (); - main_timer_1 = timer_read (); + main_timer[1] = timer_read (); /* Pwm setup. */ pwm_update (); - main_timer_2 = timer_read (); + main_timer[2] = timer_read (); /* Compute absolute position. */ postrack_update (); /* Speed control. */ if (main_mode >= 2) speed_update (); - main_timer_3 = timer_read (); + main_timer[3] = timer_read (); /* Stats. */ if (main_stat_counter && !--main_stat_counter_cpt) { @@ -160,8 +162,8 @@ main_loop (void) } if (main_stat_timer && !--main_stat_timer_cpt) { - proto_send6b ('T', main_timer_0, main_timer_2, main_timer_3, - main_timer_4, main_timer_4, main_timer_5); + proto_send6b ('T', main_timer[0], main_timer[2], main_timer[3], + main_timer[4], main_timer[4], main_timer[5]); main_stat_timer_cpt = main_stat_timer; } if (main_print_pin && !--main_print_pin_cpt) @@ -172,7 +174,7 @@ main_loop (void) /* Misc. */ while (uart0_poll ()) proto_accept (uart0_getc ()); - main_timer_4 = timer_read (); + main_timer[4] = timer_read (); } /** Handle incoming messages. */ @@ -229,22 +231,20 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_theta_cons = args[0] << 8; speed_alpha_cons = args[1] << 8; break; - case c ('s', 6): + case c ('s', 5): /* Set speed controlled position consign. * - w: theta consign offset. * - w: alpha consign offset. - * - b: theta max speed. - * - b: alpha max speed. */ - /* WARNING! This is not safe regarding proto! If the sender miss the - * acknoledgement it will send this commande twice! */ + * - b: sequence number. */ + if (args[4] == main_sequence) + break; main_mode = 2; speed_pos = 1; speed_theta_pos_cons = pos_theta_cons; speed_theta_pos_cons += v8_to_v16 (args[0], args[1]); speed_alpha_pos_cons = pos_alpha_cons; speed_alpha_pos_cons += v8_to_v16 (args[2], args[3]); - speed_theta_cons = args[4] << 8; - speed_alpha_cons = args[5] << 8; + main_sequence = args[4]; break; /* Stats. * - b: interval between stats. */ @@ -315,6 +315,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_theta_acc = v8_to_v16 (args[1], args[2]); speed_alpha_acc = v8_to_v16 (args[3], args[4]); break; + case c ('s', 3): + /* Set maximum speed. + * - b: theta. + * - b: alpha. */ + speed_theta_max = args[1]; + speed_alpha_max = args[2]; + break; case c ('p', 3): pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]); break; @@ -363,6 +370,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send1b ('E', EEPROM_KEY); proto_send1w ('f', postrack_footing); proto_send2w ('a', speed_theta_acc, speed_alpha_acc); + proto_send2b ('s', speed_theta_max, speed_alpha_max); proto_send2w ('p', pos_theta_kp, pos_alpha_kp); proto_send2w ('i', pos_theta_ki, pos_alpha_ki); proto_send2w ('d', pos_theta_kd, pos_alpha_kd); diff --git a/n/asserv/src/asserv/pwm.avr.c b/n/asserv/src/asserv/pwm.avr.c index b0db836..773cfef 100644 --- a/n/asserv/src/asserv/pwm.avr.c +++ b/n/asserv/src/asserv/pwm.avr.c @@ -40,7 +40,7 @@ int16_t pwm_left, pwm_right; /** PWM reverse direction, only set pwm dir bits or you will get weird results * on port B. */ -uint8_t pwm_dir = _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR); +uint8_t pwm_dir = _BV (PWM_LEFT_DIR); /* +AutoDec */ diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c index 324e231..eb9b568 100644 --- a/n/asserv/src/asserv/speed.c +++ b/n/asserv/src/asserv/speed.c @@ -31,8 +31,10 @@ /** Current speed, f8.8. */ int16_t speed_theta_cur, speed_alpha_cur; -/** Consign speed or maximum speed for position consign, f8.8. */ +/** Consign speed, f8.8. */ int16_t speed_theta_cons, speed_alpha_cons; +/** Maximum speed for position consign, u8. */ +int8_t speed_theta_max, speed_alpha_max; /** Consign position. */ uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; /** Weither to use the consign position (1) or not (0). */ @@ -63,17 +65,17 @@ speed_update_by_speed (void) speed_alpha_cur -= speed_alpha_acc; } -/** Compute maximum allowed speed according to: distance left, maximum speed - * consign, current speed and acceleration. */ +/** Compute maximum allowed speed according to: distance left, maximum speed, + * current speed and acceleration. */ static int16_t -speed_compute_max_speed (int32_t d, int16_t cons, int16_t cur, int16_t acc) +speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) { int16_t s; /* Compute maximum speed in order to be able to brake in time. * s = sqrt (2 * a * d) */ s = fixed_sqrt_ui32 (2 * (acc >> 8) * UTILS_ABS (d)); /* Apply consign. */ - s = UTILS_MIN (cons >> 8, s); + s = UTILS_MIN (max, s); /* Apply sign. */ if (d < 0) s = -s; @@ -89,12 +91,12 @@ speed_update_by_position (void) { speed_theta_cur = speed_compute_max_speed (speed_theta_pos_cons - pos_theta_cons, - speed_theta_cons, speed_theta_cur, - speed_theta_acc); + speed_theta_cur, speed_theta_acc, + speed_theta_max); speed_alpha_cur = speed_compute_max_speed (speed_alpha_pos_cons - pos_alpha_cons, - speed_alpha_cons, speed_alpha_cur, - speed_alpha_acc); + speed_alpha_cur, speed_alpha_acc, + speed_alpha_max); } /** Update shaft position consign according to consign. */ -- cgit v1.2.3