modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.h

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: quadramp_derivate.h,v 1.2.2.3 2007-05-23 17:18:13 zer0 Exp $
  19  *
  20  */
  21
  22 #ifndef _QUADRAMP_DERIVATE_H_
  23 #define _QUADRAMP_DERIVATE_H_
  24
  25 #include <aversive.h>
  26
  27 struct quadramp_derivate_filter
  28 {
  29     uint32_t var_2nd_ord_pos;
  30     uint32_t var_2nd_ord_neg;
  31     uint32_t var_1st_ord_pos;
  32     uint32_t var_1st_ord_neg;
  33
  34     int32_t previous_in_position;
  35     int32_t previous_out_speed;
  36
  37     int32_t goal_window;
  38     int32_t gain_anticipation; /* fixed point value, * 1/256 */
  39
  40     int32_t pivot;   /* debug only */
  41
  42     uint8_t divisor;
  43     uint8_t divisor_counter;
  44 };
  45
  46 /** Initialization of the filter
  47     as this filter has always an infinite gain, it is initilized with an integrative infinite gain limited by 1 */
  48 extern inline void quadramp_derivate_init(struct quadramp_derivate_filter * r);
  49
  50 /** set the anticipation value. This gain is a fixed point value that will be divided by 256.
  51     set this gain to have enough anticipation, so the goal is not atteined with too much speed.
  52     too much, and the goal will be overlooked, and the system will oscillate.
  53     Too less, and the goal will be atteined with speed, and the goal window will eventually cutoff brutally
  54     */
  55 extern inline void quadramp_derivate_set_gain_anticipation(struct quadramp_derivate_filter * q, uint16_t gain_anticipation);
  56
  57 /** goal window is a shutdown of the integration when the goal is atteined.
  58     this aims to get rid of the very little oscillations when immobile */
  59 extern inline void quadramp_derivate_set_goal_window(struct quadramp_derivate_filter * q, uint32_t goal_window);
  60
  61 /** as in the quadramp, we can set here the maximum speed (1st order) and maximum acceleration (2nd order)
  62     and this in both directions, positive, and negative.  */
  63 extern inline void quadramp_derivate_set_2nd_order_vars(struct quadramp_derivate_filter * q, uint32_t var_2nd_ord_pos, uint32_t var_2nd_ord_neg);
  64 extern inline void quadramp_derivate_set_1st_order_vars(struct quadramp_derivate_filter * q, uint32_t var_1st_ord_pos, uint32_t var_1st_ord_neg);
  65
  66 /** this sets a divisor. (executing only 1 time of n)
  67     this permits to make a bigger resolution on the speed and acceleration consign.
  68
  69     default is 1.
  70     When using n>1, the new acceleration (2nd order) unit is divided by n (increasing precision)
  71     The speed remains at the same unit.
  72
  73     The drawback is that the speed will have the forma of a stair, so do not abuse of it !
  74     */
  75 extern inline void quadramp_derivate_set_divisor(struct quadramp_derivate_filter * q, uint8_t divisor);
  76
  77
  78 /**
  79  * Process the ramp
  80  *
  81  * \param data should be a (struct quadramp_filter *) pointer
  82  * \param in is the input of the filter
  83  *
  84  * the input is a position (relative to goal)
  85  * The output of the function is a speed, which is typically fed as consign to a speed PID.
  86  * Beware !! the speed unit at the output must absolutely be the same that the derivate of the input,
  87  * which means that the frequency of the quadramp_derivate_do_filter must be the same than that of the speed PID.
  88  */
  89 extern int32_t quadramp_derivate_do_filter(void * data, int32_t in);
  90
  91 #endif