From d7f6cf302ce2bcedfb1eacba8dffb69877644f8c Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 25 May 2006 17:49:31 +0000 Subject: Ajout de l'ABS, l'anti-bloquage des roues. --- n/asserv/src/asserv/main.c | 4 +++- n/asserv/src/asserv/pos.c | 44 +++++++++++++++++++++++++++++++++----------- 2 files changed, 36 insertions(+), 12 deletions(-) (limited to 'n/asserv') diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index 8382b39..63fd580 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -32,7 +32,7 @@ #include "misc.h" -/** Motor command sequence. */ +/** Motor command sequence, do not use values above 127, do not use zero. */ uint8_t main_sequence, main_sequence_ack, main_sequence_finish; /* This is implementation include. */ @@ -289,6 +289,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set acknoledge. * - b: ack sequence number. */ main_sequence_ack = args[0]; + if (pos_blocked_state) + pos_reset (); break; /* Stats. * - b: interval between stats. */ diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c index 1beffc7..eecd71f 100644 --- a/n/asserv/src/asserv/pos.c +++ b/n/asserv/src/asserv/pos.c @@ -46,11 +46,16 @@ uint16_t pos_theta_kp, pos_alpha_kp; uint16_t pos_theta_ki, pos_alpha_ki; /** D coefficients. */ uint16_t pos_theta_kd, pos_alpha_kd; +/** Blocked value. If error is greater than this value, stop the robot and + * report blocked state. */ +int32_t pos_blocked = 15000L; /** Current integral values. */ int32_t pos_theta_int, pos_alpha_int; /** Last error values. */ int32_t pos_theta_e_old, pos_alpha_e_old; +/** One if blocked. */ +uint8_t pos_blocked_state; /* +AutoDec */ /* -AutoDec */ @@ -94,21 +99,37 @@ static 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. */ - pid_theta = pos_compute_pid (pos_theta_cons - pos_theta_cur, - &pos_theta_int, &pos_theta_e_old, - pos_theta_kp, pos_theta_ki, pos_theta_kd); - pid_alpha = pos_compute_pid (pos_alpha_cons - pos_alpha_cur, - &pos_alpha_int, &pos_alpha_e_old, - pos_alpha_kp, pos_alpha_ki, pos_alpha_kd); - /* 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); + diff_theta = pos_theta_cons - pos_theta_cur; + diff_alpha = pos_alpha_cons - pos_alpha_cur; + if (pos_blocked_state + || diff_theta < -pos_blocked || pos_blocked < diff_theta + || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) + { + /* Blocked. */ + pwm_left = 0; + pwm_right = 0; + pos_blocked_state = 1; + main_sequence_finish = main_sequence | 0x80; + } + else + { + pid_theta = pos_compute_pid (diff_theta, &pos_theta_int, + &pos_theta_e_old, pos_theta_kp, + pos_theta_ki, pos_theta_kd); + pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha_int, + &pos_alpha_e_old, pos_alpha_kp, + pos_alpha_ki, pos_alpha_kd); + /* 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); + } } /** Reset position control internal state. */ @@ -119,4 +140,5 @@ pos_reset (void) pos_theta_cur = pos_alpha_cur = 0; pos_theta_cons = pos_alpha_cons = 0; pos_theta_e_old = pos_alpha_e_old = 0; + pos_blocked_state = 0; } -- cgit v1.2.3