X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Fcontrol_system%2Ffilters%2Fquadramp%2Fquadramp.c;h=793df389b8b6f19229793832d9e4c3d9e01bf4c4;hp=847d01aac05fe007875250b4ab71881f55ee3faf;hb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d;hpb=ccc6954bb046671b9e28c5806db5121c1eef49c0 diff --git a/modules/devices/control_system/filters/quadramp/quadramp.c b/modules/devices/control_system/filters/quadramp/quadramp.c index 847d01a..793df38 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); @@ -76,112 +73,108 @@ uint8_t quadramp_is_finished(struct quadramp_filter *q) /** * 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; + int32_t previous_out; previous_var = q->previous_var; previous_out = q->previous_out; d = in - previous_out ; + d_float = d; /* 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 ; + previous_var = d_float ; } // update previous_out and previous_var @@ -189,5 +182,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; }