X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Fcontrol_system%2Ffilters%2Fquadramp%2Fquadramp.c;h=26d80c5660a5860ca71c9d2a614428e004933bec;hp=847d01aac05fe007875250b4ab71881f55ee3faf;hb=3b0f95e3cbbeeefef10dfba0566ef940c54b940e;hpb=ccc6954bb046671b9e28c5806db5121c1eef49c0;ds=inline diff --git a/modules/devices/control_system/filters/quadramp/quadramp.c b/modules/devices/control_system/filters/quadramp/quadramp.c index 847d01a..26d80c5 100644 --- a/modules/devices/control_system/filters/quadramp/quadramp.c +++ b/modules/devices/control_system/filters/quadramp/quadramp.c @@ -1,7 +1,6 @@ - -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * 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 @@ -27,9 +26,7 @@ #include #include -#define NEXT(n, i) (((n) + (i)/(n)) >> 1) - -void quadramp_init(struct quadramp_filter * q) +void quadramp_init(struct quadramp_filter *q) { uint8_t flags; IRQ_LOCK(flags); @@ -38,16 +35,16 @@ void quadramp_init(struct quadramp_filter * q) } -void quadramp_reset(struct quadramp_filter * q) +void quadramp_reset(struct quadramp_filter *q) { q->previous_var = 0; q->previous_out = 0; q->previous_in = 0; } -void quadramp_set_2nd_order_vars(struct quadramp_filter * q, - uint32_t var_2nd_ord_pos, - uint32_t var_2nd_ord_neg) +void quadramp_set_2nd_order_vars(struct quadramp_filter *q, + double var_2nd_ord_pos, + double var_2nd_ord_neg) { uint8_t flags; IRQ_LOCK(flags); @@ -56,9 +53,9 @@ void quadramp_set_2nd_order_vars(struct quadramp_filter * q, IRQ_UNLOCK(flags); } -void quadramp_set_1st_order_vars(struct quadramp_filter * q, - uint32_t var_1st_ord_pos, - uint32_t var_1st_ord_neg) +void quadramp_set_1st_order_vars(struct quadramp_filter *q, + double var_1st_ord_pos, + double var_1st_ord_neg) { uint8_t flags; IRQ_LOCK(flags); @@ -70,118 +67,123 @@ void quadramp_set_1st_order_vars(struct quadramp_filter * q, uint8_t quadramp_is_finished(struct quadramp_filter *q) { - return (q->previous_out == q->previous_in && + return ((int32_t)q->previous_out == q->previous_in && q->previous_var == 0); } /** * Process the ramp - * + * * \param data should be a (struct quadramp_filter *) pointer * \param in is the input of the filter - * + * */ int32_t quadramp_do_filter(void * data, int32_t in) { struct quadramp_filter * q = data; - int32_t d ; + int32_t d; /* remaining distance */ int32_t pos_target; - int32_t var_1st_ord_pos = 0; - int32_t var_1st_ord_neg = 0; - int32_t var_2nd_ord_pos = 0; - int32_t var_2nd_ord_neg = 0; - int32_t previous_var, previous_out ; - - if ( q->var_1st_ord_pos ) - var_1st_ord_pos = q->var_1st_ord_pos ; - - if ( q->var_1st_ord_neg ) - var_1st_ord_neg = -q->var_1st_ord_neg ; - - if ( q->var_2nd_ord_pos ) - var_2nd_ord_pos = q->var_2nd_ord_pos ; - - if ( q->var_2nd_ord_neg ) - var_2nd_ord_neg = -q->var_2nd_ord_neg ; + double var_1st_ord_pos = q->var_1st_ord_pos; + double var_1st_ord_neg = -q->var_1st_ord_neg; + double var_2nd_ord_pos = q->var_2nd_ord_pos; + double var_2nd_ord_neg = -q->var_2nd_ord_neg; + double previous_var, d_float; + double previous_out; previous_var = q->previous_var; previous_out = q->previous_out; - d = in - previous_out ; + d_float = (double)in - previous_out ; + + /* d is very small, we can jump to dest */ + if (fabs(d_float) < 2.) { + q->previous_var = 0; + q->previous_out = in; + q->previous_in = in; + return in; + } + + d = (int32_t)d_float; /* Deceleration ramp */ - if ( d > 0 && var_2nd_ord_neg) { - int32_t ramp_pos; + if (d > 0 && var_2nd_ord_neg) { + double ramp_pos; /* var_2nd_ord_neg < 0 */ - /* real EQ : sqrt( var_2nd_ord_neg^2/4 - 2.d.var_2nd_ord_neg ) + var_2nd_ord_neg/2 */ - ramp_pos = sqrt( (var_2nd_ord_neg*var_2nd_ord_neg)/4 - 2*d*var_2nd_ord_neg ) + var_2nd_ord_neg/2; + ramp_pos = sqrt((var_2nd_ord_neg*var_2nd_ord_neg)/4 - + 2*d_float*var_2nd_ord_neg) + + var_2nd_ord_neg/2; - if(ramp_pos < var_1st_ord_pos) - var_1st_ord_pos = ramp_pos ; + if (ramp_pos < var_1st_ord_pos) + var_1st_ord_pos = ramp_pos; } else if (d < 0 && var_2nd_ord_pos) { - int32_t ramp_neg; - + double ramp_neg; + /* var_2nd_ord_pos > 0 */ - /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */ - ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2; - + ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - + 2*d_float*var_2nd_ord_pos ) - + var_2nd_ord_pos/2; + /* ramp_neg < 0 */ - if(ramp_neg > var_1st_ord_neg) + if (ramp_neg > var_1st_ord_neg) var_1st_ord_neg = ramp_neg ; } - + /* try to set the speed : can we reach the speed with our acceleration ? */ /* si on va moins vite que la Vmax */ - if ( previous_var < var_1st_ord_pos ) { + if (previous_var < var_1st_ord_pos) { /* acceleration would be to high, we reduce the speed */ /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax, * on sature Vmax a Vcourante + acceleration */ - if (var_2nd_ord_pos && ( var_1st_ord_pos - previous_var > var_2nd_ord_pos) ) - var_1st_ord_pos = previous_var + var_2nd_ord_pos ; + if (var_2nd_ord_pos && + (var_1st_ord_pos - previous_var) > var_2nd_ord_pos) + var_1st_ord_pos = previous_var + var_2nd_ord_pos; } /* si on va plus vite que Vmax */ - else if ( previous_var > var_1st_ord_pos ) { + else if (previous_var > var_1st_ord_pos) { /* deceleration would be to high, we increase the speed */ /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax, * on sature Vmax a Vcourante + deceleration */ - if (var_2nd_ord_neg && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) ) + if (var_2nd_ord_neg && + (var_1st_ord_pos - previous_var) < var_2nd_ord_neg) var_1st_ord_pos = previous_var + var_2nd_ord_neg; } - + /* same for the neg */ /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */ - if ( previous_var > var_1st_ord_neg ) { + if (previous_var > var_1st_ord_neg) { /* acceleration would be to high, we reduce the speed */ /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin, * on sature Vmax a Vcourante + deceleration */ - if (var_2nd_ord_neg && ( var_1st_ord_neg - previous_var < var_2nd_ord_neg) ) - var_1st_ord_neg = previous_var + var_2nd_ord_neg ; + if (var_2nd_ord_neg && + (var_1st_ord_neg - previous_var) < var_2nd_ord_neg) + var_1st_ord_neg = previous_var + var_2nd_ord_neg; } /* si on va moins vite que Vmin (mais vitesse absolue superieure) */ - else if ( previous_var < var_1st_ord_neg ) { + else if (previous_var < var_1st_ord_neg) { /* deceleration would be to high, we increase the speed */ /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin, * on sature Vmax a Vcourante + deceleration */ - if (var_2nd_ord_pos && (var_1st_ord_neg - previous_var > var_2nd_ord_pos) ) + if (var_2nd_ord_pos && + (var_1st_ord_neg - previous_var) > var_2nd_ord_pos) var_1st_ord_neg = previous_var + var_2nd_ord_pos; } /* * Position consign : can we reach the position with our speed ? */ - if ( /* var_1st_ord_pos && */d > var_1st_ord_pos ) { + if (d_float > var_1st_ord_pos) { pos_target = previous_out + var_1st_ord_pos ; previous_var = var_1st_ord_pos ; } - else if ( /* var_1st_ord_neg && */d < var_1st_ord_neg ) { + else if (d_float < var_1st_ord_neg) { pos_target = previous_out + var_1st_ord_neg ; previous_var = var_1st_ord_neg ; } else { - pos_target = previous_out + d ; - previous_var = d ; + pos_target = in; + previous_var = d_float ; } // update previous_out and previous_var @@ -189,5 +191,5 @@ int32_t quadramp_do_filter(void * data, int32_t in) q->previous_out = pos_target; q->previous_in = in; - return pos_target ; + return pos_target; }