3 * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 * Revision : $Id: quadramp.c,v 1.4.4.7 2009-05-18 12:29:51 zer0 Exp $
30 #define NEXT(n, i) (((n) + (i)/(n)) >> 1)
32 void quadramp_init(struct quadramp_filter * q)
36 memset(q, 0, sizeof(*q));
41 void quadramp_reset(struct quadramp_filter * q)
48 void quadramp_set_2nd_order_vars(struct quadramp_filter * q,
49 uint32_t var_2nd_ord_pos,
50 uint32_t var_2nd_ord_neg)
54 q->var_2nd_ord_pos = var_2nd_ord_pos;
55 q->var_2nd_ord_neg = var_2nd_ord_neg;
59 void quadramp_set_1st_order_vars(struct quadramp_filter * q,
60 uint32_t var_1st_ord_pos,
61 uint32_t var_1st_ord_neg)
65 q->var_1st_ord_pos = var_1st_ord_pos;
66 q->var_1st_ord_neg = var_1st_ord_neg;
71 uint8_t quadramp_is_finished(struct quadramp_filter *q)
73 return (q->previous_out == q->previous_in &&
74 q->previous_var == 0);
80 * \param data should be a (struct quadramp_filter *) pointer
81 * \param in is the input of the filter
84 int32_t quadramp_do_filter(void * data, int32_t in)
86 struct quadramp_filter * q = data;
89 int32_t var_1st_ord_pos = 0;
90 int32_t var_1st_ord_neg = 0;
91 int32_t var_2nd_ord_pos = 0;
92 int32_t var_2nd_ord_neg = 0;
93 int32_t previous_var, previous_out ;
95 if ( q->var_1st_ord_pos )
96 var_1st_ord_pos = q->var_1st_ord_pos ;
98 if ( q->var_1st_ord_neg )
99 var_1st_ord_neg = -q->var_1st_ord_neg ;
101 if ( q->var_2nd_ord_pos )
102 var_2nd_ord_pos = q->var_2nd_ord_pos ;
104 if ( q->var_2nd_ord_neg )
105 var_2nd_ord_neg = -q->var_2nd_ord_neg ;
107 previous_var = q->previous_var;
108 previous_out = q->previous_out;
110 d = in - previous_out ;
112 /* Deceleration ramp */
113 if ( d > 0 && var_2nd_ord_neg) {
115 /* var_2nd_ord_neg < 0 */
116 /* real EQ : sqrt( var_2nd_ord_neg^2/4 - 2.d.var_2nd_ord_neg ) + var_2nd_ord_neg/2 */
117 ramp_pos = sqrt( (var_2nd_ord_neg*var_2nd_ord_neg)/4 - 2*d*var_2nd_ord_neg ) + var_2nd_ord_neg/2;
119 if(ramp_pos < var_1st_ord_pos)
120 var_1st_ord_pos = ramp_pos ;
123 else if (d < 0 && var_2nd_ord_pos) {
126 /* var_2nd_ord_pos > 0 */
127 /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */
128 ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2;
131 if(ramp_neg > var_1st_ord_neg)
132 var_1st_ord_neg = ramp_neg ;
135 /* try to set the speed : can we reach the speed with our acceleration ? */
136 /* si on va moins vite que la Vmax */
137 if ( previous_var < var_1st_ord_pos ) {
138 /* acceleration would be to high, we reduce the speed */
139 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
140 * on sature Vmax a Vcourante + acceleration */
141 if (var_2nd_ord_pos && ( var_1st_ord_pos - previous_var > var_2nd_ord_pos) )
142 var_1st_ord_pos = previous_var + var_2nd_ord_pos ;
144 /* si on va plus vite que Vmax */
145 else if ( previous_var > var_1st_ord_pos ) {
146 /* deceleration would be to high, we increase the speed */
147 /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax,
148 * on sature Vmax a Vcourante + deceleration */
149 if (var_2nd_ord_neg && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) )
150 var_1st_ord_pos = previous_var + var_2nd_ord_neg;
153 /* same for the neg */
154 /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */
155 if ( previous_var > var_1st_ord_neg ) {
156 /* acceleration would be to high, we reduce the speed */
157 /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin,
158 * on sature Vmax a Vcourante + deceleration */
159 if (var_2nd_ord_neg && ( var_1st_ord_neg - previous_var < var_2nd_ord_neg) )
160 var_1st_ord_neg = previous_var + var_2nd_ord_neg ;
162 /* si on va moins vite que Vmin (mais vitesse absolue superieure) */
163 else if ( previous_var < var_1st_ord_neg ) {
164 /* deceleration would be to high, we increase the speed */
165 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
166 * on sature Vmax a Vcourante + deceleration */
167 if (var_2nd_ord_pos && (var_1st_ord_neg - previous_var > var_2nd_ord_pos) )
168 var_1st_ord_neg = previous_var + var_2nd_ord_pos;
172 * Position consign : can we reach the position with our speed ?
174 if ( /* var_1st_ord_pos && */d > var_1st_ord_pos ) {
175 pos_target = previous_out + var_1st_ord_pos ;
176 previous_var = var_1st_ord_pos ;
178 else if ( /* var_1st_ord_neg && */d < var_1st_ord_neg ) {
179 pos_target = previous_out + var_1st_ord_neg ;
180 previous_var = var_1st_ord_neg ;
183 pos_target = previous_out + d ;
187 // update previous_out and previous_var
188 q->previous_var = previous_var;
189 q->previous_out = pos_target;