2 * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: quadramp.c,v 1.4.4.7 2009-05-18 12:29:51 zer0 Exp $
29 void quadramp_init(struct quadramp_filter *q)
33 memset(q, 0, sizeof(*q));
38 void quadramp_reset(struct quadramp_filter *q)
45 void quadramp_set_2nd_order_vars(struct quadramp_filter *q,
46 double var_2nd_ord_pos,
47 double var_2nd_ord_neg)
51 q->var_2nd_ord_pos = var_2nd_ord_pos;
52 q->var_2nd_ord_neg = var_2nd_ord_neg;
56 void quadramp_set_1st_order_vars(struct quadramp_filter *q,
57 double var_1st_ord_pos,
58 double var_1st_ord_neg)
62 q->var_1st_ord_pos = var_1st_ord_pos;
63 q->var_1st_ord_neg = var_1st_ord_neg;
68 uint8_t quadramp_is_finished(struct quadramp_filter *q)
70 return ((int32_t)q->previous_out == q->previous_in &&
71 q->previous_var == 0);
77 * \param data should be a (struct quadramp_filter *) pointer
78 * \param in is the input of the filter
81 int32_t quadramp_do_filter(void * data, int32_t in)
83 struct quadramp_filter * q = data;
84 int32_t d; /* remaining distance */
86 double var_1st_ord_pos = q->var_1st_ord_pos;
87 double var_1st_ord_neg = -q->var_1st_ord_neg;
88 double var_2nd_ord_pos = q->var_2nd_ord_pos;
89 double var_2nd_ord_neg = -q->var_2nd_ord_neg;
90 double previous_var, d_float;
93 previous_var = q->previous_var;
94 previous_out = q->previous_out;
96 d_float = (double)in - previous_out ;
98 /* d is very small, we can jump to dest */
99 if (fabs(d_float) < 2.) {
101 q->previous_out = in;
106 d = (int32_t)d_float;
108 /* Deceleration ramp */
109 if (d > 0 && var_2nd_ord_neg) {
111 /* var_2nd_ord_neg < 0 */
112 ramp_pos = sqrt((var_2nd_ord_neg*var_2nd_ord_neg)/4 -
113 2*d_float*var_2nd_ord_neg) +
116 if (ramp_pos < var_1st_ord_pos)
117 var_1st_ord_pos = ramp_pos;
120 else if (d < 0 && var_2nd_ord_pos) {
123 /* var_2nd_ord_pos > 0 */
124 ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 -
125 2*d_float*var_2nd_ord_pos ) -
129 if (ramp_neg > var_1st_ord_neg)
130 var_1st_ord_neg = ramp_neg ;
133 /* try to set the speed : can we reach the speed with our acceleration ? */
134 /* si on va moins vite que la Vmax */
135 if (previous_var < var_1st_ord_pos) {
136 /* acceleration would be to high, we reduce the speed */
137 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
138 * on sature Vmax a Vcourante + acceleration */
139 if (var_2nd_ord_pos &&
140 (var_1st_ord_pos - previous_var) > var_2nd_ord_pos)
141 var_1st_ord_pos = previous_var + var_2nd_ord_pos;
143 /* si on va plus vite que Vmax */
144 else if (previous_var > var_1st_ord_pos) {
145 /* deceleration would be to high, we increase the speed */
146 /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax,
147 * on sature Vmax a Vcourante + deceleration */
148 if (var_2nd_ord_neg &&
149 (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 &&
160 (var_1st_ord_neg - previous_var) < var_2nd_ord_neg)
161 var_1st_ord_neg = previous_var + var_2nd_ord_neg;
163 /* si on va moins vite que Vmin (mais vitesse absolue superieure) */
164 else if (previous_var < var_1st_ord_neg) {
165 /* deceleration would be to high, we increase the speed */
166 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
167 * on sature Vmax a Vcourante + deceleration */
168 if (var_2nd_ord_pos &&
169 (var_1st_ord_neg - previous_var) > var_2nd_ord_pos)
170 var_1st_ord_neg = previous_var + var_2nd_ord_pos;
174 * Position consign : can we reach the position with our speed ?
176 if (d_float > var_1st_ord_pos) {
177 pos_target = previous_out + var_1st_ord_pos ;
178 previous_var = var_1st_ord_pos ;
180 else if (d_float < var_1st_ord_neg) {
181 pos_target = previous_out + var_1st_ord_neg ;
182 previous_var = var_1st_ord_neg ;
186 previous_var = d_float ;
189 // update previous_out and previous_var
190 q->previous_var = previous_var;
191 q->previous_out = pos_target;