From 804ae2cb383eff9a617443512fb7571797156e0d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 Mar 2008 00:27:31 +0100 Subject: * digital/asserv/src/asserv: - added auxiliary motor position and speed control. --- digital/asserv/src/asserv/eeprom.avr.c | 17 ++++++ digital/asserv/src/asserv/eeprom.h | 4 +- digital/asserv/src/asserv/main.c | 94 ++++++++++++++++++++++++++++------ digital/asserv/src/asserv/pos.c | 75 ++++++++++++++++++--------- digital/asserv/src/asserv/pos.h | 1 + digital/asserv/src/asserv/speed.c | 30 ++++++++--- digital/asserv/src/asserv/speed.h | 1 + 7 files changed, 173 insertions(+), 49 deletions(-) diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index dbbdbdec..96611d3b 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -34,6 +34,11 @@ #define EEPROM_START 0 +/* WARNING: + * If you change EEPROM format, be sure to change the EEPROM_KEY in header if + * your new format is not compatible with the old one or you will load + * garbages in parameters. */ + /* Read parameters from eeprom. */ void eeprom_read_params (void) @@ -44,19 +49,25 @@ eeprom_read_params (void) return; speed_theta.max = eeprom_read_byte (p8++); speed_alpha.max = eeprom_read_byte (p8++); + speed_aux0.max = eeprom_read_byte (p8++); speed_theta.slow = eeprom_read_byte (p8++); speed_alpha.slow = eeprom_read_byte (p8++); + speed_aux0.slow = eeprom_read_byte (p8++); pwm_set_reverse (eeprom_read_byte (p8++)); p16 = (uint16_t *) p8; postrack_set_footing (eeprom_read_word (p16++)); speed_theta.acc = eeprom_read_word (p16++); speed_alpha.acc = eeprom_read_word (p16++); + speed_aux0.acc = eeprom_read_word (p16++); pos_theta.kp = eeprom_read_word (p16++); pos_alpha.kp = eeprom_read_word (p16++); + pos_aux0.kp = eeprom_read_word (p16++); pos_theta.ki = eeprom_read_word (p16++); pos_alpha.ki = eeprom_read_word (p16++); + pos_aux0.ki = eeprom_read_word (p16++); pos_theta.kd = eeprom_read_word (p16++); pos_alpha.kd = eeprom_read_word (p16++); + pos_aux0.kd = eeprom_read_word (p16++); pos_e_sat = eeprom_read_word (p16++); pos_int_sat = eeprom_read_word (p16++); pos_blocked = eeprom_read_word (p16++); @@ -71,19 +82,25 @@ eeprom_write_params (void) eeprom_write_byte (p8++, EEPROM_KEY); eeprom_write_byte (p8++, speed_theta.max); eeprom_write_byte (p8++, speed_alpha.max); + eeprom_write_byte (p8++, speed_aux0.max); eeprom_write_byte (p8++, speed_theta.slow); eeprom_write_byte (p8++, speed_alpha.slow); + eeprom_write_byte (p8++, speed_aux0.slow); eeprom_write_byte (p8++, pwm_reverse); p16 = (uint16_t *) p8; eeprom_write_word (p16++, postrack_footing); eeprom_write_word (p16++, speed_theta.acc); eeprom_write_word (p16++, speed_alpha.acc); + eeprom_write_word (p16++, speed_aux0.acc); eeprom_write_word (p16++, pos_theta.kp); eeprom_write_word (p16++, pos_alpha.kp); + eeprom_write_word (p16++, pos_aux0.kp); eeprom_write_word (p16++, pos_theta.ki); eeprom_write_word (p16++, pos_alpha.ki); + eeprom_write_word (p16++, pos_aux0.ki); eeprom_write_word (p16++, pos_theta.kd); eeprom_write_word (p16++, pos_alpha.kd); + eeprom_write_word (p16++, pos_aux0.kd); eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_int_sat); eeprom_write_word (p16++, pos_blocked); diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index 929d0da6..95bb4e64 100644 --- a/digital/asserv/src/asserv/eeprom.h +++ b/digital/asserv/src/asserv/eeprom.h @@ -25,8 +25,8 @@ * * }}} */ -/* Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x45 +/** Change the eeprom key each time you change eeprom format. */ +#define EEPROM_KEY 0x46 void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 6d761315..38a2e9f8 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -59,6 +59,9 @@ uint8_t main_stat_counter, main_stat_counter_cpt; /** Report of position. */ uint8_t main_stat_postrack, main_stat_postrack_cpt; +/** Report of auxiliary position. */ +uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt; + /** Statistics about speed control. */ uint8_t main_stat_speed, main_stat_speed_cpt; @@ -122,8 +125,7 @@ main_loop (void) counter_update (); main_timer[0] = timer_read (); /* Position control. */ - if (state_main.mode >= MODE_POS) - pos_update (); + pos_update (); main_timer[1] = timer_read (); /* Pwm setup. */ pwm_update (); @@ -134,8 +136,7 @@ main_loop (void) if (state_main.mode >= MODE_TRAJ) traj_update (); /* Speed control. */ - if (state_main.mode >= MODE_SPEED) - speed_update (); + speed_update (); main_timer[3] = timer_read (); /* Stats. */ if (main_sequence_ack @@ -156,15 +157,22 @@ main_loop (void) proto_send3d ('X', postrack_x, postrack_y, postrack_a); main_stat_postrack_cpt = main_stat_postrack; } + if (main_stat_aux_pos && !--main_stat_aux_pos_cpt) + { + proto_send1w ('Z', pos_aux0.cur); + main_stat_aux_pos_cpt = main_stat_aux_pos; + } if (main_stat_speed && !--main_stat_speed_cpt) { - proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8); + proto_send3b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8, + speed_aux0.cur >> 8); main_stat_speed_cpt = main_stat_speed; } if (main_stat_pos && !--main_stat_pos_cpt) { - proto_send4w ('P', pos_theta.e_old, pos_theta.i, - pos_alpha.e_old, pos_alpha.i); + proto_send6w ('P', pos_theta.e_old, pos_theta.i, + pos_alpha.e_old, pos_alpha.i, + pos_aux0.e_old, pos_aux0.i); main_stat_pos_cpt = main_stat_pos; } #ifdef HOST @@ -235,7 +243,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('w', 2): /* Set auxiliary pwm. * - w: pwm. */ - //pos_reset (); // TODO + pos_reset (&pos_aux0); state_aux0.mode = MODE_PWM; pwm_aux0 = v8_to_v16 (args[0], args[1]); UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX); @@ -248,6 +256,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pos_theta.cons += v8_to_v16 (args[0], args[1]); pos_alpha.cons += v8_to_v16 (args[2], args[3]); break; + case c ('c', 2): + /* Add to auxiliary position consign. + * - w: consign offset. */ + state_aux0.mode = MODE_POS; + pos_aux0.cons += v8_to_v16 (args[0], args[1]); + break; case c ('s', 0): /* Stop (set zero speed). */ state_main.mode = MODE_SPEED; @@ -264,6 +278,13 @@ 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', 1): + /* Set auxiliary speed. + * - b: speed. */ + state_aux0.mode = MODE_SPEED; + speed_aux0.use_pos = 0; + speed_aux0.cons = args[0] << 8; + break; case c ('s', 9): /* Set speed controlled position consign. * - d: theta consign offset. @@ -279,6 +300,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); state_start (&state_main, args[8]); break; + case c ('s', 5): + /* Set auxiliary speed controlled position consign. + * - d: consign offset. + * - b: sequence number. */ + if (args[4] == state_aux0.sequence) + break; + state_aux0.mode = MODE_SPEED; + speed_aux0.use_pos = 1; + speed_aux0.pos_cons = pos_aux0.cons; + speed_aux0.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); + state_start (&state_aux0, args[4]); + break; case c ('f', 1): /* Go to the wall. * - b: sequence number. */ @@ -289,10 +322,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) traj_mode = 10; state_start (&state_main, args[0]); break; + case c ('a', 2): + /* Set both acknoledge. + * - b: main ack sequence number. + * - b: auxiliary ack sequence number. */ + if (state_aux0.blocked && args[1] == state_aux0.finished) + pos_reset (&pos_aux0); + state_acknowledge (&state_aux0, args[1]); + /* no break; */ case c ('a', 1): - /* Set acknoledge. - * - b: ack sequence number. */ - if (state_main.blocked) + /* Set main acknoledge. + * - b: main ack sequence number. */ + if (state_main.blocked && args[0] == state_main.finished) { pos_reset (&pos_theta); pos_reset (&pos_alpha); @@ -313,6 +354,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Position stats. */ main_stat_postrack_cpt = main_stat_postrack = args[0]; break; + case c ('Z', 1): + /* Auxiliary position stats. */ + main_stat_aux_pos_cpt = main_stat_aux_pos = args[0]; + break; case c ('S', 1): /* Motor speed control stats. */ main_stat_speed_cpt = main_stat_speed = args[0]; @@ -372,14 +417,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) postrack_set_footing (v8_to_v16 (args[1], args[2])); break; case c ('a', 5): - /* Set acceleration. + /* Set main acceleration. * - w: theta. * - w: alpha. */ speed_theta.acc = v8_to_v16 (args[1], args[2]); speed_alpha.acc = v8_to_v16 (args[3], args[4]); break; + case c ('a', 3): + /* Set auxiliary acceleration. + * - w: acceleration. */ + speed_aux0.acc = v8_to_v16 (args[1], args[2]); + break; case c ('s', 5): - /* Set maximum and slow speed. + /* Set main maximum and slow speed. * - b: theta max. * - b: alpha max. * - b: theta slow. @@ -389,22 +439,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed_theta.slow = args[3]; speed_alpha.slow = args[4]; break; + case c ('s', 3): + /* Set auxiliary maximum and slow speed. + * - b: speed max. + * - b: speed slow. */ + speed_aux0.max = args[1]; + speed_aux0.slow = args[2]; + break; case c ('p', 3): - pos_theta.kp = pos_alpha.kp = v8_to_v16 (args[1], args[2]); + pos_aux0.kp = v8_to_v16 (args[1], args[2]); break; case c ('p', 5): pos_theta.kp = v8_to_v16 (args[1], args[2]); pos_alpha.kp = v8_to_v16 (args[3], args[4]); break; case c ('i', 3): - pos_theta.ki = pos_alpha.ki = v8_to_v16 (args[1], args[2]); + pos_aux0.ki = v8_to_v16 (args[1], args[2]); break; case c ('i', 5): pos_theta.ki = v8_to_v16 (args[1], args[2]); pos_alpha.ki = v8_to_v16 (args[3], args[4]); break; case c ('d', 3): - pos_theta.kd = pos_alpha.kd = v8_to_v16 (args[1], args[2]); + pos_aux0.kd = v8_to_v16 (args[1], args[2]); break; case c ('d', 5): pos_theta.kd = v8_to_v16 (args[1], args[2]); @@ -442,6 +499,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) 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); + proto_send1w ('a', speed_aux0.acc); + proto_send2b ('s', speed_aux0.max, speed_aux0.slow); + proto_send1w ('p', pos_aux0.kp); + proto_send1w ('i', pos_aux0.ki); + proto_send1w ('d', pos_aux0.kd); proto_send1w ('E', pos_e_sat); proto_send1w ('I', pos_int_sat); proto_send1w ('b', pos_blocked); diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index e241b0fb..091335e2 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -43,6 +43,9 @@ /** Theta/alpha control states. */ struct pos_t pos_theta, pos_alpha; +/** Auxiliaries control states. */ +struct pos_t pos_aux0; + /** Error saturation. */ int32_t pos_e_sat = 1023; /** Integral saturation. */ @@ -88,32 +91,58 @@ pos_compute_pid (int32_t e, struct pos_t *pos) void pos_update (void) { - int16_t pid_theta, pid_alpha; - int32_t diff_theta, diff_alpha; - /* Update current shaft positions. */ - pos_theta.cur += counter_left_diff + counter_right_diff; - pos_alpha.cur += counter_right_diff - counter_left_diff; - /* Compute PID. */ - diff_theta = pos_theta.cons - pos_theta.cur; - diff_alpha = pos_alpha.cons - pos_alpha.cur; - if (state_main.blocked - || diff_theta < -pos_blocked || pos_blocked < diff_theta - || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) + if (state_main.mode >= MODE_POS) { - /* Blocked. */ - pwm_left = 0; - pwm_right = 0; - state_blocked (&state_main); + int16_t pid_theta, pid_alpha; + int32_t diff_theta, diff_alpha; + /* Update current shaft positions. */ + pos_theta.cur += counter_left_diff + counter_right_diff; + pos_alpha.cur += counter_right_diff - counter_left_diff; + /* Compute PID. */ + diff_theta = pos_theta.cons - pos_theta.cur; + diff_alpha = pos_alpha.cons - pos_alpha.cur; + if (state_main.blocked + || diff_theta < -pos_blocked || pos_blocked < diff_theta + || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) + { + /* Blocked. */ + pwm_left = 0; + pwm_right = 0; + state_blocked (&state_main); + } + else + { + pid_theta = pos_compute_pid (diff_theta, &pos_theta); + pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha); + /* Update PWM. */ + pwm_left = pid_theta - pid_alpha; + UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); + pwm_right = pid_theta + pid_alpha; + UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); + } } - else + if (state_aux0.mode >= MODE_POS) { - pid_theta = pos_compute_pid (diff_theta, &pos_theta); - pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha); - /* Update PWM. */ - pwm_left = pid_theta - pid_alpha; - UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); - pwm_right = pid_theta + pid_alpha; - UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); + int16_t pid; + int32_t diff; + /* Update current shaft position. */ + pos_aux0.cur += counter_aux0_diff; + /* Compute PID. */ + diff = pos_aux0.cons - pos_aux0.cur; + if (state_aux0.blocked + || diff < -pos_blocked || pos_blocked < diff) + { + /* Blocked. */ + pwm_aux0 = 0; + state_blocked (&state_aux0); + } + else + { + pid = pos_compute_pid (diff, &pos_aux0); + /* Update PWM. */ + pwm_aux0 = pid; + UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX); + } } } diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h index 1859d178..d28db334 100644 --- a/digital/asserv/src/asserv/pos.h +++ b/digital/asserv/src/asserv/pos.h @@ -41,6 +41,7 @@ struct pos_t }; extern struct pos_t pos_theta, pos_alpha; +extern struct pos_t pos_aux0; extern int32_t pos_e_sat; extern int32_t pos_int_sat; diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index b50a6b52..679ce416 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -40,6 +40,9 @@ /** Theta/alpha speed control states. */ struct speed_t speed_theta, speed_alpha; +/** Auxiliaries speed control states. */ +struct speed_t speed_aux0; + /** Update shaft position consign according to a speed consign. */ static void speed_update_by_speed (struct speed_t *speed) @@ -101,15 +104,26 @@ speed_update_by (struct speed_t *speed, struct pos_t *pos) void speed_update (void) { - /* Update speed. */ - speed_update_by (&speed_theta, &pos_theta); - speed_update_by (&speed_alpha, &pos_alpha); - /* Check for completion. */ - if ((speed_theta.use_pos || speed_alpha.use_pos) - && (!speed_theta.use_pos || speed_theta.cur == 0) - && (!speed_alpha.use_pos || speed_alpha.cur == 0)) + if (state_main.mode >= MODE_SPEED) + { + /* Update speed. */ + speed_update_by (&speed_theta, &pos_theta); + speed_update_by (&speed_alpha, &pos_alpha); + /* Check for completion. */ + if ((speed_theta.use_pos || speed_alpha.use_pos) + && (!speed_theta.use_pos || speed_theta.cur == 0) + && (!speed_alpha.use_pos || speed_alpha.cur == 0)) + { + state_finish (&state_main); + } + } + if (state_aux0.mode >= MODE_SPEED) { - state_finish (&state_main); + /* Update speed. */ + speed_update_by (&speed_aux0, &pos_aux0); + /* Check for completion. */ + if (speed_aux0.use_pos && speed_aux0.cur == 0) + state_finish (&state_aux0); } } diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h index f62f267f..d5df91a2 100644 --- a/digital/asserv/src/asserv/speed.h +++ b/digital/asserv/src/asserv/speed.h @@ -45,6 +45,7 @@ struct speed_t }; extern struct speed_t speed_theta, speed_alpha; +extern struct speed_t speed_aux0; void speed_update (void); -- cgit v1.2.3