From 4895e54c666db9720abda0aa8ae901058679aee3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 18 Dec 2011 15:19:06 +0100 Subject: digital/asserv: convert to new control system --- digital/asserv/src/asserv/eeprom.avr.c | 194 ++++++++++++++++++--------------- 1 file changed, 108 insertions(+), 86 deletions(-) (limited to 'digital/asserv/src/asserv/eeprom.avr.c') diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index ddc10f3c..004a2ecb 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -29,10 +29,7 @@ #include -#include "counter.h" -#include "pwm.h" -#include "pos.h" -#include "speed.h" +#include "cs.h" #include "postrack.h" #include "traj.h" @@ -71,49 +68,62 @@ eeprom_read_params (void) 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++); - speed_aux[0].max = eeprom_read_byte (p8++); - speed_aux[1].max = eeprom_read_byte (p8++); - speed_theta.slow = eeprom_read_byte (p8++); - speed_alpha.slow = eeprom_read_byte (p8++); - speed_aux[0].slow = eeprom_read_byte (p8++); - speed_aux[1].slow = eeprom_read_byte (p8++); - pwm_set_reverse (eeprom_read_byte (p8++)); - counter_right_correction = eeprom_read_32 (p8); p8 += 4; + cs_main.speed_theta.max = eeprom_read_byte (p8++); + cs_main.speed_alpha.max = eeprom_read_byte (p8++); + cs_aux[0].speed.max = eeprom_read_byte (p8++); + cs_aux[1].speed.max = eeprom_read_byte (p8++); + cs_main.speed_theta.slow = eeprom_read_byte (p8++); + cs_main.speed_alpha.slow = eeprom_read_byte (p8++); + cs_aux[0].speed.slow = eeprom_read_byte (p8++); + cs_aux[1].speed.slow = eeprom_read_byte (p8++); + output_set_reverse (&output_left, eeprom_read_byte (p8++)); + output_set_reverse (&output_right, eeprom_read_byte (p8++)); + output_set_reverse (&output_aux[0], eeprom_read_byte (p8++)); + output_set_reverse (&output_aux[1], eeprom_read_byte (p8++)); + encoder_corrector_set_correction (&encoder_right_corrector, + eeprom_read_32 (p8)); p8 += 4; 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_aux[0].acc = eeprom_read_word (p16++); - speed_aux[1].acc = eeprom_read_word (p16++); - pos_theta.kp = eeprom_read_word (p16++); - pos_alpha.kp = eeprom_read_word (p16++); - pos_aux[0].kp = eeprom_read_word (p16++); - pos_aux[1].kp = eeprom_read_word (p16++); - pos_theta.ki = eeprom_read_word (p16++); - pos_alpha.ki = eeprom_read_word (p16++); - pos_aux[0].ki = eeprom_read_word (p16++); - pos_aux[1].ki = eeprom_read_word (p16++); - pos_theta.kd = eeprom_read_word (p16++); - pos_alpha.kd = eeprom_read_word (p16++); - pos_aux[0].kd = eeprom_read_word (p16++); - pos_aux[1].kd = eeprom_read_word (p16++); - pos_theta.blocked_error_limit = eeprom_read_word (p16++); - pos_theta.blocked_speed_limit = eeprom_read_word (p16++); - pos_theta.blocked_counter_limit = eeprom_read_word (p16++); - pos_alpha.blocked_error_limit = eeprom_read_word (p16++); - pos_alpha.blocked_speed_limit = eeprom_read_word (p16++); - pos_alpha.blocked_counter_limit = eeprom_read_word (p16++); - pos_aux[0].blocked_error_limit = eeprom_read_word (p16++); - pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++); - pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++); - pos_aux[1].blocked_error_limit = eeprom_read_word (p16++); - pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++); - pos_aux[1].blocked_counter_limit = eeprom_read_word (p16++); - pos_e_sat = eeprom_read_word (p16++); - pos_i_sat = eeprom_read_word (p16++); - pos_d_sat = eeprom_read_word (p16++); + cs_main.speed_theta.acc = eeprom_read_word (p16++); + cs_main.speed_alpha.acc = eeprom_read_word (p16++); + cs_aux[0].speed.acc = eeprom_read_word (p16++); + cs_aux[1].speed.acc = eeprom_read_word (p16++); + cs_main.pos_theta.kp = eeprom_read_word (p16++); + cs_main.pos_alpha.kp = eeprom_read_word (p16++); + cs_aux[0].pos.kp = eeprom_read_word (p16++); + cs_aux[1].pos.kp = eeprom_read_word (p16++); + cs_main.pos_theta.ki = eeprom_read_word (p16++); + cs_main.pos_alpha.ki = eeprom_read_word (p16++); + cs_aux[0].pos.ki = eeprom_read_word (p16++); + cs_aux[1].pos.ki = eeprom_read_word (p16++); + cs_main.pos_theta.kd = eeprom_read_word (p16++); + cs_main.pos_alpha.kd = eeprom_read_word (p16++); + cs_aux[0].pos.kd = eeprom_read_word (p16++); + cs_aux[1].pos.kd = eeprom_read_word (p16++); + cs_main.blocking_detection_theta.error_limit = eeprom_read_word (p16++); + cs_main.blocking_detection_theta.speed_limit = eeprom_read_word (p16++); + cs_main.blocking_detection_theta.counter_limit = eeprom_read_word (p16++); + cs_main.blocking_detection_alpha.error_limit = eeprom_read_word (p16++); + cs_main.blocking_detection_alpha.speed_limit = eeprom_read_word (p16++); + cs_main.blocking_detection_alpha.counter_limit = eeprom_read_word (p16++); + cs_aux[0].blocking_detection.error_limit = eeprom_read_word (p16++); + cs_aux[0].blocking_detection.speed_limit = eeprom_read_word (p16++); + cs_aux[0].blocking_detection.counter_limit = eeprom_read_word (p16++); + cs_aux[1].blocking_detection.error_limit = eeprom_read_word (p16++); + cs_aux[1].blocking_detection.speed_limit = eeprom_read_word (p16++); + cs_aux[1].blocking_detection.counter_limit = eeprom_read_word (p16++); + cs_main.pos_theta.e_sat = eeprom_read_word (p16++); + cs_main.pos_theta.i_sat = eeprom_read_word (p16++); + cs_main.pos_theta.d_sat = eeprom_read_word (p16++); + cs_main.pos_alpha.e_sat = eeprom_read_word (p16++); + cs_main.pos_alpha.i_sat = eeprom_read_word (p16++); + cs_main.pos_alpha.d_sat = eeprom_read_word (p16++); + cs_aux[0].pos.e_sat = eeprom_read_word (p16++); + cs_aux[0].pos.i_sat = eeprom_read_word (p16++); + cs_aux[0].pos.d_sat = eeprom_read_word (p16++); + cs_aux[1].pos.e_sat = eeprom_read_word (p16++); + cs_aux[1].pos.i_sat = eeprom_read_word (p16++); + cs_aux[1].pos.d_sat = eeprom_read_word (p16++); traj_eps = eeprom_read_word (p16++); traj_aeps = eeprom_read_word (p16++); traj_set_angle_limit (eeprom_read_word (p16++)); @@ -126,49 +136,61 @@ eeprom_write_params (void) 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++, speed_aux[0].max); - eeprom_write_byte (p8++, speed_aux[1].max); - eeprom_write_byte (p8++, speed_theta.slow); - eeprom_write_byte (p8++, speed_alpha.slow); - eeprom_write_byte (p8++, speed_aux[0].slow); - eeprom_write_byte (p8++, speed_aux[1].slow); - eeprom_write_byte (p8++, pwm_reverse); - eeprom_write_32 (p8, counter_right_correction); p8 += 4; + eeprom_write_byte (p8++, cs_main.speed_theta.max); + eeprom_write_byte (p8++, cs_main.speed_alpha.max); + eeprom_write_byte (p8++, cs_aux[0].speed.max); + eeprom_write_byte (p8++, cs_aux[1].speed.max); + eeprom_write_byte (p8++, cs_main.speed_theta.slow); + eeprom_write_byte (p8++, cs_main.speed_alpha.slow); + eeprom_write_byte (p8++, cs_aux[0].speed.slow); + eeprom_write_byte (p8++, cs_aux[1].speed.slow); + eeprom_write_byte (p8++, output_left.reverse); + eeprom_write_byte (p8++, output_right.reverse); + eeprom_write_byte (p8++, output_aux[0].reverse); + eeprom_write_byte (p8++, output_aux[1].reverse); + eeprom_write_32 (p8, encoder_right_corrector.correction); p8 += 4; 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_aux[0].acc); - eeprom_write_word (p16++, speed_aux[1].acc); - eeprom_write_word (p16++, pos_theta.kp); - eeprom_write_word (p16++, pos_alpha.kp); - eeprom_write_word (p16++, pos_aux[0].kp); - eeprom_write_word (p16++, pos_aux[1].kp); - eeprom_write_word (p16++, pos_theta.ki); - eeprom_write_word (p16++, pos_alpha.ki); - eeprom_write_word (p16++, pos_aux[0].ki); - eeprom_write_word (p16++, pos_aux[1].ki); - eeprom_write_word (p16++, pos_theta.kd); - eeprom_write_word (p16++, pos_alpha.kd); - eeprom_write_word (p16++, pos_aux[0].kd); - eeprom_write_word (p16++, pos_aux[1].kd); - eeprom_write_word (p16++, pos_theta.blocked_error_limit); - eeprom_write_word (p16++, pos_theta.blocked_speed_limit); - eeprom_write_word (p16++, pos_theta.blocked_counter_limit); - eeprom_write_word (p16++, pos_alpha.blocked_error_limit); - eeprom_write_word (p16++, pos_alpha.blocked_speed_limit); - eeprom_write_word (p16++, pos_alpha.blocked_counter_limit); - eeprom_write_word (p16++, pos_aux[0].blocked_error_limit); - eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit); - eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit); - eeprom_write_word (p16++, pos_aux[1].blocked_error_limit); - eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit); - eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit); - eeprom_write_word (p16++, pos_e_sat); - eeprom_write_word (p16++, pos_i_sat); - eeprom_write_word (p16++, pos_d_sat); + eeprom_write_word (p16++, cs_main.speed_theta.acc); + eeprom_write_word (p16++, cs_main.speed_alpha.acc); + eeprom_write_word (p16++, cs_aux[0].speed.acc); + eeprom_write_word (p16++, cs_aux[1].speed.acc); + eeprom_write_word (p16++, cs_main.pos_theta.kp); + eeprom_write_word (p16++, cs_main.pos_alpha.kp); + eeprom_write_word (p16++, cs_aux[0].pos.kp); + eeprom_write_word (p16++, cs_aux[1].pos.kp); + eeprom_write_word (p16++, cs_main.pos_theta.ki); + eeprom_write_word (p16++, cs_main.pos_alpha.ki); + eeprom_write_word (p16++, cs_aux[0].pos.ki); + eeprom_write_word (p16++, cs_aux[1].pos.ki); + eeprom_write_word (p16++, cs_main.pos_theta.kd); + eeprom_write_word (p16++, cs_main.pos_alpha.kd); + eeprom_write_word (p16++, cs_aux[0].pos.kd); + eeprom_write_word (p16++, cs_aux[1].pos.kd); + eeprom_write_word (p16++, cs_main.blocking_detection_theta.error_limit); + eeprom_write_word (p16++, cs_main.blocking_detection_theta.speed_limit); + eeprom_write_word (p16++, cs_main.blocking_detection_theta.counter_limit); + eeprom_write_word (p16++, cs_main.blocking_detection_alpha.error_limit); + eeprom_write_word (p16++, cs_main.blocking_detection_alpha.speed_limit); + eeprom_write_word (p16++, cs_main.blocking_detection_alpha.counter_limit); + eeprom_write_word (p16++, cs_aux[0].blocking_detection.error_limit); + eeprom_write_word (p16++, cs_aux[0].blocking_detection.speed_limit); + eeprom_write_word (p16++, cs_aux[0].blocking_detection.counter_limit); + eeprom_write_word (p16++, cs_aux[1].blocking_detection.error_limit); + eeprom_write_word (p16++, cs_aux[1].blocking_detection.speed_limit); + eeprom_write_word (p16++, cs_aux[1].blocking_detection.counter_limit); + eeprom_write_word (p16++, cs_main.pos_theta.e_sat); + eeprom_write_word (p16++, cs_main.pos_theta.i_sat); + eeprom_write_word (p16++, cs_main.pos_theta.d_sat); + eeprom_write_word (p16++, cs_main.pos_alpha.e_sat); + eeprom_write_word (p16++, cs_main.pos_alpha.i_sat); + eeprom_write_word (p16++, cs_main.pos_alpha.d_sat); + eeprom_write_word (p16++, cs_aux[0].pos.e_sat); + eeprom_write_word (p16++, cs_aux[0].pos.i_sat); + eeprom_write_word (p16++, cs_aux[0].pos.d_sat); + eeprom_write_word (p16++, cs_aux[1].pos.e_sat); + eeprom_write_word (p16++, cs_aux[1].pos.i_sat); + eeprom_write_word (p16++, cs_aux[1].pos.d_sat); eeprom_write_word (p16++, traj_eps); eeprom_write_word (p16++, traj_aeps); eeprom_write_word (p16++, traj_angle_limit); -- cgit v1.2.3