/* speed.c - PI speed control. */ /* asserv - Position & speed motor control on a ATmega128. {{{ * * Copyright (C) 2004 Nicolas Schodet * * Robot APB Team/Efrei 2005. * Web: http://assos.efrei.fr/robot/ * Email: robot AT efrei DOT fr * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ /** Actual speed. */ 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_l = 0x08, speed_max_a = 0x05; /** Acceleration value. */ uint8_t speed_acc = 0x60; /** Acceleration counter, speed gets updated when it reachs 0. */ uint8_t speed_acc_cpt; /** Error saturation. */ int16_t speed_e_sat = 1023; /** Integral saturation. */ int16_t speed_int_sat = 8191; /** P coeficients. 4.8 fixed point format. */ uint16_t speed_left_kp = 0x0fff, speed_right_kp = 0x0fff; /** I coeficients. 3.8 fixed point format. */ uint16_t speed_left_ki = 0x0008, speed_right_ki = 0x0008; /** D coeficients. 3.8 fixed point format. */ uint16_t speed_left_kd = 0x0002, speed_right_kd = 0x0002; /** D sample period. */ uint8_t speed_ds = 2; /** Integral term. */ int16_t speed_left_int, speed_right_int; /** Last error value. */ int16_t speed_left_e_old, speed_right_e_old; /** D history. */ int16_t speed_left_d_hist[16], speed_right_d_hist[16]; /** D history pointer. */ uint8_t speed_d_hist_ptr; /** D history sum. */ int16_t speed_left_d_hist_sum, speed_right_d_hist_sum; /** Counter value wanted. */ uint16_t speed_left_counter, speed_right_counter; /** Do not update counter. */ uint8_t speed_counter_updated = 0; /* +AutoDec */ /** Initialise speed parameters. */ static inline void speed_init (void); /** Update speeds according to the wanted speeds and the acceleration. */ static inline void speed_update (void); /** Compute new pwm value for left motor. */ static inline void speed_compute_left_pwm (void); /** Compute new pwm value for right motor. */ static inline void speed_compute_right_pwm (void); /** Forget past event, usefull when the speed control is disabled for some * time. */ 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. */ static inline void speed_init (void) { } /** Update speeds according to the wanted speeds and the acceleration. */ static inline void speed_update (void) { /* Accelerate. */ if (speed_acc) { speed_acc_cpt--; if (speed_acc_cpt == 0) { speed_acc_cpt = speed_acc; /* Update speeds. */ if (speed_left > speed_left_aim) speed_left--; else if (speed_left < speed_left_aim) speed_left++; if (speed_right > speed_right_aim) speed_right--; else if (speed_right < speed_right_aim) speed_right++; } } else { speed_left = speed_left_aim; speed_right = speed_right_aim; } /* Update counter. */ if (speed_counter_updated) { speed_counter_updated = 0; } else { 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. */ static inline void speed_compute_left_pwm (void) { int16_t e, diff, pwm; /* Get and saturate error. */ 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; 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; 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; /* Compute PI. */ /* 16b = 15b + 14b + 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; } /** Compute new pwm value for right motor. */ static inline void speed_compute_right_pwm (void) { int16_t e, diff, pwm; /* Get and saturate error. */ 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; 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; 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_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; } /** Forget past event, usefull when the speed control is disabled for some * time. */ static inline void speed_restart (void) { int i; speed_left_int = 0; speed_right_int = 0; speed_left = 0; speed_right = 0; speed_left_counter = counter_left; speed_right_counter = counter_right; speed_left_d_hist_sum = 0; speed_right_d_hist_sum = 0; speed_d_hist_ptr = 0; for (i = 0; i < sizeof (speed_left_d_hist) / sizeof (speed_left_d_hist[0]); i++) { speed_left_d_hist[i] = 0; speed_right_d_hist[i] = 0; } } /** 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) { /* XXX: There are some buggy assertions there if dist and arc are both * big. */ uint8_t vls, vas; uint32_t vl248, va248; int8_t vl, va; /* Drop sign. */ vls = dist < 0; if (vls) dist = -dist; vas = arc < 0; if (vas) arc = -arc; /* Compute speed. * v = sqrt (2 * a * d), a = 1/speed_acc */ vl248 = dsp_sqrt_f248 (dist * 2 / speed_acc); vl = (vl248 >> 8) & 0xff; va248 = dsp_sqrt_f248 (arc * 2 / speed_acc); va = (va248 >> 8) & 0xff; /* Saturate. */ 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; speed_left_aim = vl - va; 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; }