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_derivate.c,v 1.2.2.4 2007-11-24 22:12:07 zer0 Exp $
25 #include <quadramp_derivate.h>
28 void quadramp_derivate_init(struct quadramp_derivate_filter * q)
33 q->var_2nd_ord_pos = 1;
34 q->var_2nd_ord_neg = 1;
35 q->var_1st_ord_pos = 0;
36 q->var_1st_ord_neg = 0;
38 q->previous_in_position = 0;
39 q->previous_out_speed = 0;
41 q-> gain_anticipation= 0;
52 void quadramp_derivate_set_gain_anticipation(struct quadramp_derivate_filter * q, uint16_t gain_anticipation)
56 q->gain_anticipation = gain_anticipation;
60 void quadramp_derivate_set_goal_window(struct quadramp_derivate_filter * q, uint32_t goal_window)
64 q->goal_window = goal_window;
68 void quadramp_derivate_set_2nd_order_vars(struct quadramp_derivate_filter * q, uint32_t var_2nd_ord_pos, uint32_t var_2nd_ord_neg)
72 q->var_2nd_ord_pos = var_2nd_ord_pos;
73 q->var_2nd_ord_neg = var_2nd_ord_neg;
77 void quadramp_derivate_set_1st_order_vars(struct quadramp_derivate_filter * q, uint32_t var_1st_ord_pos, uint32_t var_1st_ord_neg)
81 q->var_1st_ord_pos = var_1st_ord_pos;
82 q->var_1st_ord_neg = var_1st_ord_neg;
87 void quadramp_derivate_set_divisor(struct quadramp_derivate_filter * q, uint8_t divisor)
93 q->divisor_counter = 1;
102 * \param data should be a (struct quadramp_derivate_filter *) pointer
103 * \param in is the input of the filter
106 int32_t quadramp_derivate_do_filter(void * data, int32_t in_position)
108 struct quadramp_derivate_filter * q = data;
109 int32_t position_pivot, speed, var_2nd_ord, acceleration_consign, speed_consign;
112 this is a state machine who executes the algorithm only one time out of "divisor" */
113 if( q->divisor != 1) {
114 if (-- (q->divisor_counter) !=0 ) {
115 // if it is not time to exec the algorithm, we just test the goal_window
116 if(ABS( in_position ) < q->goal_window)
117 q->previous_out_speed =0;
118 // and return the previous consign
119 return q->previous_out_speed;
122 q->divisor_counter = q->divisor;
127 /** compensation of the inversion before the input
128 (inversion of the control system where error = consign - feedback)
130 in_position = -in_position;
133 // calculating the actual speed (derivate)
134 speed = in_position - q->previous_in_position;
138 /** limitation of this speed, due to overflows, and calculations based on theoretical max value
139 and also the peak created when the position_consign changes */
141 if(q->var_1st_ord_pos)
142 MAX(speed , (q->var_1st_ord_pos * q-> divisor) ); // divisor reequilibrates the value.
145 if(q->var_1st_ord_neg)
146 MIN(speed , (-(q->var_1st_ord_neg* q-> divisor)) ); // divisor reequilibrates the value.
152 /** calculation of the pivot position.
153 when this position is atteined, it is just the right time to begin to deccelerate.
154 The length to this position is given by a linear decceleration to 0 : x = speedĀ²/ (2 * acceleration)
158 // taking the concerned acc. value
159 if (speed >=0) // why not position ?
160 var_2nd_ord = q->var_2nd_ord_pos;
162 var_2nd_ord = q->var_2nd_ord_neg;
164 // anticipation, proportionnal to speed. Gain_anticipation is a fixed point value, with 8 bits shift
165 position_pivot = (ABS(speed) * q->gain_anticipation) >>8 ;
167 // if necessary, compensation of the output units, when using a sampler divisor
168 if(q->divisor != 1) {
169 var_2nd_ord *= q-> divisor;
170 position_pivot /= q-> divisor;
173 // pivot calculation itself
174 position_pivot += speed*speed /(2*var_2nd_ord);
176 // taking the right sign
178 position_pivot = - position_pivot;
180 // mem only for debug
181 q-> pivot = position_pivot;
183 /** this is the heart of the trajectory generation.
184 Pretty simple but indeed unstable,
185 because of this corresponds to an infinite gain, in the following equation :
186 acceleration_consign = ( position_pivot - in_position ) * gain
188 In fact this unstability is erased by the fact that the acc value is nearly always limited
190 if(position_pivot >= in_position)
191 acceleration_consign = q->var_2nd_ord_pos;
193 acceleration_consign = -q->var_2nd_ord_neg;
197 /** integration and limitation of the acceleration to obtain a speed consign */
198 speed_consign = q->previous_out_speed + acceleration_consign;
200 if (speed_consign >=0) {
201 if(q->var_1st_ord_pos)
202 MAX(speed_consign , q->var_1st_ord_pos);
205 if(q->var_1st_ord_neg)
206 MIN(speed_consign , -q->var_1st_ord_neg);
210 /** creation of an end arrival window. This is done to stop the oscillations when the goal is achieved. */
211 if(ABS( in_position ) < q->goal_window)
214 /** refresh the memories */
215 q->previous_in_position = in_position;
216 q->previous_out_speed = speed_consign;
219 return speed_consign ;