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 #define NEXT(n, i)  (((n) + (i)/(n)) >> 1)
  30
  31 void quadramp_init(struct quadramp_filter * q)
  32 {
  33         uint8_t flags;
  34         IRQ_LOCK(flags);
  35         memset(q, 0, sizeof(*q));
  36         IRQ_UNLOCK(flags);
  37 }
  38
  39
  40 void quadramp_reset(struct quadramp_filter * q)
  41 {
  42         q->previous_var = 0;
  43         q->previous_out = 0;
  44         q->previous_in = 0;
  45 }
  46
  47 void quadramp_set_2nd_order_vars(struct quadramp_filter * q,
  48                                  uint32_t var_2nd_ord_pos,
  49                                  uint32_t var_2nd_ord_neg)
  50 {
  51         uint8_t flags;
  52         IRQ_LOCK(flags);
  53         q->var_2nd_ord_pos = var_2nd_ord_pos;
  54         q->var_2nd_ord_neg = var_2nd_ord_neg;
  55         IRQ_UNLOCK(flags);
  56 }
  57
  58 void quadramp_set_1st_order_vars(struct quadramp_filter * q,
  59                                  uint32_t var_1st_ord_pos,
  60                                  uint32_t var_1st_ord_neg)
  61 {
  62         uint8_t flags;
  63         IRQ_LOCK(flags);
  64         q->var_1st_ord_pos = var_1st_ord_pos;
  65         q->var_1st_ord_neg = var_1st_ord_neg;
  66         IRQ_UNLOCK(flags);
  67 }
  68
  69
  70 uint8_t quadramp_is_finished(struct quadramp_filter *q)
  71 {
  72         return (q->previous_out == q->previous_in &&
  73                 q->previous_var == 0);
  74 }
  75
  76 /**
  77  * Process the ramp
  78  *
  79  * \param data should be a (struct quadramp_filter *) pointer
  80  * \param in is the input of the filter
  81  *
  82  */
  83 int32_t quadramp_do_filter(void * data, int32_t in)
  84 {
  85         struct quadramp_filter * q = data;
  86         int32_t d ;
  87         int32_t pos_target;
  88         int32_t var_1st_ord_pos = 0;
  89         int32_t var_1st_ord_neg = 0;
  90         int32_t var_2nd_ord_pos = 0;
  91         int32_t var_2nd_ord_neg = 0;
  92         int32_t previous_var, previous_out ;
  93
  94         if ( q->var_1st_ord_pos )
  95                 var_1st_ord_pos = q->var_1st_ord_pos ;
  96
  97         if ( q->var_1st_ord_neg )
  98                 var_1st_ord_neg = -q->var_1st_ord_neg ;
  99
 100         if ( q->var_2nd_ord_pos )
 101                 var_2nd_ord_pos = q->var_2nd_ord_pos ;
 102
 103         if ( q->var_2nd_ord_neg )
 104                 var_2nd_ord_neg = -q->var_2nd_ord_neg ;
 105
 106         previous_var = q->previous_var;
 107         previous_out = q->previous_out;
 108
 109         d = in - previous_out ;
 110
 111         /* Deceleration ramp */
 112         if ( d > 0 && var_2nd_ord_neg) {
 113                 int32_t ramp_pos;
 114                 /* var_2nd_ord_neg < 0 */
 115                 /* real EQ : sqrt( var_2nd_ord_neg^2/4 - 2.d.var_2nd_ord_neg ) + var_2nd_ord_neg/2 */
 116                 ramp_pos = sqrt( (var_2nd_ord_neg*var_2nd_ord_neg)/4 - 2*d*var_2nd_ord_neg ) + var_2nd_ord_neg/2;
 117
 118                 if(ramp_pos < var_1st_ord_pos)
 119                         var_1st_ord_pos = ramp_pos ;
 120         }
 121
 122         else if (d < 0 && var_2nd_ord_pos) {
 123                 int32_t ramp_neg;
 124
 125                 /* var_2nd_ord_pos > 0 */
 126                 /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */
 127                 ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2;
 128
 129                 /* ramp_neg < 0 */
 130                 if(ramp_neg > var_1st_ord_neg)
 131                         var_1st_ord_neg = ramp_neg ;
 132         }
 133
 134         /* try to set the speed : can we reach the speed with our acceleration ? */
 135         /* si on va moins vite que la Vmax */
 136         if ( previous_var < var_1st_ord_pos )  {
 137                 /* acceleration would be to high, we reduce the speed */
 138                 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
 139                  * on sature Vmax a Vcourante + acceleration */
 140                 if (var_2nd_ord_pos && ( 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 && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) )
 149                         var_1st_ord_pos = previous_var + var_2nd_ord_neg;
 150         }
 151
 152         /* same for the neg */
 153         /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */
 154         if ( previous_var > var_1st_ord_neg )  {
 155                 /* acceleration would be to high, we reduce the speed */
 156                 /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin,
 157                  * on sature Vmax a Vcourante + deceleration */
 158                 if (var_2nd_ord_neg && ( var_1st_ord_neg - previous_var < var_2nd_ord_neg) )
 159                         var_1st_ord_neg = previous_var + var_2nd_ord_neg ;
 160         }
 161         /* si on va moins vite que Vmin (mais vitesse absolue superieure) */
 162         else if ( previous_var < var_1st_ord_neg )  {
 163                 /* deceleration would be to high, we increase the speed */
 164                 /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
 165                  * on sature Vmax a Vcourante + deceleration */
 166                 if (var_2nd_ord_pos && (var_1st_ord_neg - previous_var > var_2nd_ord_pos) )
 167                         var_1st_ord_neg = previous_var + var_2nd_ord_pos;
 168         }
 169
 170         /*
 171          * Position consign : can we reach the position with our speed ?
 172          */
 173         if ( /* var_1st_ord_pos &&  */d > var_1st_ord_pos ) {
 174                 pos_target = previous_out + var_1st_ord_pos ;
 175                 previous_var = var_1st_ord_pos ;
 176         }
 177         else if ( /* var_1st_ord_neg &&  */d < var_1st_ord_neg ) {
 178                 pos_target = previous_out + var_1st_ord_neg ;
 179                 previous_var = var_1st_ord_neg ;
 180         }
 181         else {
 182                 pos_target = previous_out + d ;
 183                 previous_var = d ;
 184         }
 185
 186         // update previous_out and previous_var
 187         q->previous_var = previous_var;
 188         q->previous_out = pos_target;
 189         q->previous_in = in;
 190
 191         return pos_target ;
 192 }