modules/devices/control_system/filters/quadramp/quadramp.c

   1
   2 /*
   3  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   4  *
   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.
   9  *
  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.
  14  *
  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
  18  *
  19  *  Revision : $Id: quadramp.c,v 1.4.4.7 2009-05-18 12:29:51 zer0 Exp $
  20  *
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include <aversive.h>
  28 #include <quadramp.h>
  29
  30 #define NEXT(n, i)  (((n) + (i)/(n)) >> 1)
  31
  32 void quadramp_init(struct quadramp_filter * q)
  33 {
  34         uint8_t flags;
  35         IRQ_LOCK(flags);
  36         memset(q, 0, sizeof(*q));
  37         IRQ_UNLOCK(flags);
  38 }
  39
  40
  41 void quadramp_reset(struct quadramp_filter * q)
  42 {
  43         q->previous_var = 0;
  44         q->previous_out = 0;
  45         q->previous_in = 0;
  46 }
  47
  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)
  51 {
  52         uint8_t flags;
  53         IRQ_LOCK(flags);
  54         q->var_2nd_ord_pos = var_2nd_ord_pos;
  55         q->var_2nd_ord_neg = var_2nd_ord_neg;
  56         IRQ_UNLOCK(flags);
  57 }
  58
  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)
  62 {
  63         uint8_t flags;
  64         IRQ_LOCK(flags);
  65         q->var_1st_ord_pos = var_1st_ord_pos;
  66         q->var_1st_ord_neg = var_1st_ord_neg;
  67         IRQ_UNLOCK(flags);
  68 }
  69
  70
  71 uint8_t quadramp_is_finished(struct quadramp_filter *q)
  72 {
  73         return (q->previous_out == q->previous_in &&
  74                 q->previous_var == 0);
  75 }
  76
  77 /**
  78  * Process the ramp
  79  *
  80  * \param data should be a (struct quadramp_filter *) pointer
  81  * \param in is the input of the filter
  82  *
  83  */
  84 int32_t quadramp_do_filter(void * data, int32_t in)
  85 {
  86         struct quadramp_filter * q = data;
  87         int32_t d ;
  88         int32_t pos_target;
  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 ;
  94
  95         if ( q->var_1st_ord_pos )
  96                 var_1st_ord_pos = q->var_1st_ord_pos ;
  97
  98         if ( q->var_1st_ord_neg )
  99                 var_1st_ord_neg = -q->var_1st_ord_neg ;
 100
 101         if ( q->var_2nd_ord_pos )
 102                 var_2nd_ord_pos = q->var_2nd_ord_pos ;
 103
 104         if ( q->var_2nd_ord_neg )
 105                 var_2nd_ord_neg = -q->var_2nd_ord_neg ;
 106
 107         previous_var = q->previous_var;
 108         previous_out = q->previous_out;
 109
 110         d = in - previous_out ;
 111
 112         /* Deceleration ramp */
 113         if ( d > 0 && var_2nd_ord_neg) {
 114                 int32_t ramp_pos;
 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;
 118
 119                 if(ramp_pos < var_1st_ord_pos)
 120                         var_1st_ord_pos = ramp_pos ;
 121         }
 122
 123         else if (d < 0 && var_2nd_ord_pos) {
 124                 int32_t ramp_neg;
 125
 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;
 129
 130                 /* ramp_neg < 0 */
 131                 if(ramp_neg > var_1st_ord_neg)
 132                         var_1st_ord_neg = ramp_neg ;
 133         }
 134
 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 ;
 143         }
 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;
 151         }
 152
 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 ;
 161         }
 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;
 169         }
 170
 171         /*
 172          * Position consign : can we reach the position with our speed ?
 173          */
 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 ;
 177         }
 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 ;
 181         }
 182         else {
 183                 pos_target = previous_out + d ;
 184                 previous_var = d ;
 185         }
 186
 187         // update previous_out and previous_var
 188         q->previous_var = previous_var;
 189         q->previous_out = pos_target;
 190         q->previous_in = in;
 191
 192         return pos_target ;
 193 }