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

   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.c,v 1.4.4.7 2009-05-18 12:29:51 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include <aversive.h>
  27 #include <quadramp.h>
  28
  29 void quadramp_init(struct quadramp_filter *q)
  30 {
  31         uint8_t flags;
  32         IRQ_LOCK(flags);
  33         memset(q, 0, sizeof(*q));
  34         IRQ_UNLOCK(flags);
  35 }
  36
  37
  38 void quadramp_reset(struct quadramp_filter *q)
  39 {
  40         q->previous_var = 0;
  41         q->previous_out = 0;
  42         q->previous_in = 0;
  43 }
  44
  45 void quadramp_set_2nd_order_vars(struct quadramp_filter *q,
  46                                  double var_2nd_ord_pos,
  47                                  double var_2nd_ord_neg)
  48 {
  49         uint8_t flags;
  50         IRQ_LOCK(flags);
  51         q->var_2nd_ord_pos = var_2nd_ord_pos;
  52         q->var_2nd_ord_neg = var_2nd_ord_neg;
  53         IRQ_UNLOCK(flags);
  54 }
  55
  56 void quadramp_set_1st_order_vars(struct quadramp_filter *q,
  57                                  double var_1st_ord_pos,
  58                                  double var_1st_ord_neg)
  59 {
  60         uint8_t flags;
  61         IRQ_LOCK(flags);
  62         q->var_1st_ord_pos = var_1st_ord_pos;
  63         q->var_1st_ord_neg = var_1st_ord_neg;
  64         IRQ_UNLOCK(flags);
  65 }
  66
  67
  68 uint8_t quadramp_is_finished(struct quadramp_filter *q)
  69 {
  70         return ((int32_t)q->previous_out == q->previous_in &&
  71                 q->previous_var == 0);
  72 }
  73
  74 /**
  75  * Process the ramp
  76  *
  77  * \param data should be a (struct quadramp_filter *) pointer
  78  * \param in is the input of the filter
  79  *
  80  */
  81 int32_t quadramp_do_filter(void * data, int32_t in)
  82 {
  83         struct quadramp_filter * q = data;
  84         int32_t d; /* remaining distance */
  85         int32_t pos_target;
  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;
  91         double previous_out;
  92
  93         previous_var = q->previous_var;
  94         previous_out = q->previous_out;
  95
  96         d_float = (double)in - previous_out ;
  97
  98         /* d is very small, we can jump to dest */
  99         if (fabs(d_float) < 2.) {
 100                 q->previous_var = 0;
 101                 q->previous_out = in;
 102                 q->previous_in = in;
 103                 return in;
 104         }
 105
 106         d = (int32_t)d_float;
 107
 108         /* Deceleration ramp */
 109         if (d > 0 && var_2nd_ord_neg) {
 110                 double ramp_pos;
 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) +
 114                         var_2nd_ord_neg/2;
 115
 116                 if (ramp_pos < var_1st_ord_pos)
 117                         var_1st_ord_pos = ramp_pos;
 118         }
 119
 120         else if (d < 0 && var_2nd_ord_pos) {
 121                 double ramp_neg;
 122
 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 ) -
 126                         var_2nd_ord_pos/2;
 127
 128                 /* ramp_neg < 0 */
 129                 if (ramp_neg > var_1st_ord_neg)
 130                         var_1st_ord_neg = ramp_neg ;
 131         }
 132
 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;
 142         }
 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;
 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 &&
 160                     (var_1st_ord_neg - previous_var) < var_2nd_ord_neg)
 161                         var_1st_ord_neg = previous_var + var_2nd_ord_neg;
 162         }
 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;
 171         }
 172
 173         /*
 174          * Position consign : can we reach the position with our speed ?
 175          */
 176         if (d_float > var_1st_ord_pos) {
 177                 pos_target = previous_out + var_1st_ord_pos ;
 178                 previous_var = var_1st_ord_pos ;
 179         }
 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 ;
 183         }
 184         else {
 185                 pos_target = in;
 186                 previous_var = d_float ;
 187         }
 188
 189         // update previous_out and previous_var
 190         q->previous_var = previous_var;
 191         q->previous_out = pos_target;
 192         q->previous_in = in;
 193
 194         return pos_target;
 195 }