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 (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         int32_t previous_out;
  92
  93         previous_var = q->previous_var;
  94         previous_out = q->previous_out;
  95
  96         d = in - previous_out ;
  97         d_float = d;
  98
  99         /* Deceleration ramp */
 100         if (d > 0 && var_2nd_ord_neg) {
 101                 double ramp_pos;
 102                 /* var_2nd_ord_neg < 0 */
 103                 ramp_pos = sqrt((var_2nd_ord_neg*var_2nd_ord_neg)/4 -
 104                                 2*d_float*var_2nd_ord_neg) +
 105                         var_2nd_ord_neg/2;
 106
 107                 if (ramp_pos < var_1st_ord_pos)
 108                         var_1st_ord_pos = ramp_pos;
 109         }
 110
 111         else if (d < 0 && var_2nd_ord_pos) {
 112                 double ramp_neg;
 113
 114                 /* var_2nd_ord_pos > 0 */
 115                 ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 -
 116                                   2*d_float*var_2nd_ord_pos ) -
 117                         var_2nd_ord_pos/2;
 118
 119                 /* ramp_neg < 0 */
 120                 if (ramp_neg > var_1st_ord_neg)
 121                         var_1st_ord_neg = ramp_neg ;
 122         }
 123
 124         /* try to set the speed : can we reach the speed with our acceleration ? */
 125         /* si on va moins vite que la Vmax */
 126         if (previous_var < var_1st_ord_pos)  {
 127                 /* acceleration would be to high, we reduce the speed */
 128                 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
 129                  * on sature Vmax a Vcourante + acceleration */
 130                 if (var_2nd_ord_pos &&
 131                     (var_1st_ord_pos - previous_var) > var_2nd_ord_pos)
 132                         var_1st_ord_pos = previous_var + var_2nd_ord_pos;
 133         }
 134         /* si on va plus vite que Vmax */
 135         else if (previous_var > var_1st_ord_pos)  {
 136                 /* deceleration would be to high, we increase the speed */
 137                 /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax,
 138                  * on sature Vmax a Vcourante + deceleration */
 139                 if (var_2nd_ord_neg &&
 140                     (var_1st_ord_pos - previous_var) < var_2nd_ord_neg)
 141                         var_1st_ord_pos = previous_var + var_2nd_ord_neg;
 142         }
 143
 144         /* same for the neg */
 145         /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */
 146         if (previous_var > var_1st_ord_neg)  {
 147                 /* acceleration would be to high, we reduce the speed */
 148                 /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin,
 149                  * on sature Vmax a Vcourante + deceleration */
 150                 if (var_2nd_ord_neg &&
 151                     (var_1st_ord_neg - previous_var) < var_2nd_ord_neg)
 152                         var_1st_ord_neg = previous_var + var_2nd_ord_neg;
 153         }
 154         /* si on va moins vite que Vmin (mais vitesse absolue superieure) */
 155         else if (previous_var < var_1st_ord_neg)  {
 156                 /* deceleration would be to high, we increase the speed */
 157                 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
 158                  * on sature Vmax a Vcourante + deceleration */
 159                 if (var_2nd_ord_pos &&
 160                     (var_1st_ord_neg - previous_var) > var_2nd_ord_pos)
 161                         var_1st_ord_neg = previous_var + var_2nd_ord_pos;
 162         }
 163
 164         /*
 165          * Position consign : can we reach the position with our speed ?
 166          */
 167         if (d_float > var_1st_ord_pos) {
 168                 pos_target = previous_out + var_1st_ord_pos ;
 169                 previous_var = var_1st_ord_pos ;
 170         }
 171         else if (d_float < var_1st_ord_neg) {
 172                 pos_target = previous_out + var_1st_ord_neg ;
 173                 previous_var = var_1st_ord_neg ;
 174         }
 175         else {
 176                 pos_target = previous_out + d ;
 177                 previous_var = d_float ;
 178         }
 179
 180         // update previous_out and previous_var
 181         q->previous_var = previous_var;
 182         q->previous_out = pos_target;
 183         q->previous_in = in;
 184
 185         return pos_target;
 186 }