modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.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_derivate.c,v 1.2.2.4 2007-11-24 22:12:07 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23
  24 #include <aversive.h>
  25 #include <quadramp_derivate.h>
  26
  27
  28 void quadramp_derivate_init(struct quadramp_derivate_filter * q)
  29 {
  30         uint8_t flags;
  31         IRQ_LOCK(flags);
  32
  33         q->var_2nd_ord_pos = 1;
  34         q->var_2nd_ord_neg = 1;
  35         q->var_1st_ord_pos = 0;
  36         q->var_1st_ord_neg = 0;
  37
  38         q->previous_in_position = 0;
  39         q->previous_out_speed = 0;
  40
  41         q-> gain_anticipation= 0;
  42         q-> goal_window= 0;
  43
  44         q-> divisor = 1;
  45
  46         q-> pivot = 0;
  47
  48         IRQ_UNLOCK(flags);
  49 }
  50
  51
  52 void quadramp_derivate_set_gain_anticipation(struct quadramp_derivate_filter * q, uint16_t gain_anticipation)
  53 {
  54         uint8_t flags;
  55         IRQ_LOCK(flags);
  56         q->gain_anticipation = gain_anticipation;
  57         IRQ_UNLOCK(flags);
  58 }
  59
  60 void quadramp_derivate_set_goal_window(struct quadramp_derivate_filter * q, uint32_t goal_window)
  61 {
  62         uint8_t flags;
  63         IRQ_LOCK(flags);
  64         q->goal_window = goal_window;
  65         IRQ_UNLOCK(flags);
  66 }
  67
  68 void quadramp_derivate_set_2nd_order_vars(struct quadramp_derivate_filter * q, uint32_t var_2nd_ord_pos, uint32_t var_2nd_ord_neg)
  69 {
  70         uint8_t flags;
  71         IRQ_LOCK(flags);
  72         q->var_2nd_ord_pos = var_2nd_ord_pos;
  73         q->var_2nd_ord_neg = var_2nd_ord_neg;
  74         IRQ_UNLOCK(flags);
  75 }
  76
  77 void quadramp_derivate_set_1st_order_vars(struct quadramp_derivate_filter * q, uint32_t var_1st_ord_pos, uint32_t var_1st_ord_neg)
  78 {
  79         uint8_t flags;
  80         IRQ_LOCK(flags);
  81         q->var_1st_ord_pos = var_1st_ord_pos;
  82         q->var_1st_ord_neg = var_1st_ord_neg;
  83         IRQ_UNLOCK(flags);
  84 }
  85
  86
  87 void quadramp_derivate_set_divisor(struct quadramp_derivate_filter * q, uint8_t divisor)
  88 {
  89         uint8_t flags;
  90         IRQ_LOCK(flags);
  91
  92         q->divisor = divisor;
  93         q->divisor_counter = 1;
  94
  95         IRQ_UNLOCK(flags);
  96 }
  97
  98
  99 /**
 100  * Process the ramp
 101  *
 102  * \param data should be a (struct quadramp_derivate_filter *) pointer
 103  * \param in is the input of the filter
 104  *
 105  */
 106 int32_t quadramp_derivate_do_filter(void * data, int32_t in_position)
 107 {
 108         struct quadramp_derivate_filter * q = data;
 109         int32_t position_pivot, speed, var_2nd_ord, acceleration_consign, speed_consign;
 110
 111         /** sampling divisor
 112             this is a state machine who executes the algorithm only one time out of "divisor" */
 113         if( q->divisor != 1) {
 114                 if (-- (q->divisor_counter) !=0 ) {
 115                         // if it is not time to exec the algorithm, we just test the goal_window
 116                         if(ABS( in_position ) < q->goal_window)
 117                                 q->previous_out_speed =0;
 118                         // and return the previous consign
 119                         return q->previous_out_speed;
 120                 }
 121
 122                 q->divisor_counter = q->divisor;
 123         }
 124
 125
 126
 127         /** compensation of the inversion before the input
 128             (inversion of the control system where error = consign - feedback)
 129         */
 130         in_position = -in_position;
 131
 132
 133         // calculating the actual speed (derivate)
 134         speed = in_position - q->previous_in_position;
 135
 136
 137
 138         /** limitation of this speed, due to overflows, and calculations based on theoretical max value
 139             and also the peak created when the position_consign changes */
 140         if (speed >=0) {
 141                 if(q->var_1st_ord_pos)
 142                         MAX(speed , (q->var_1st_ord_pos * q-> divisor) ); // divisor reequilibrates the value.
 143         }
 144         else {
 145                 if(q->var_1st_ord_neg)
 146                         MIN(speed , (-(q->var_1st_ord_neg* q-> divisor)) ); // divisor reequilibrates the value.
 147         }
 148
 149
 150
 151
 152         /** calculation of the pivot position.
 153             when this position is atteined, it is just the right time to begin to deccelerate.
 154             The length to this position is given by a linear decceleration to 0 : x = speed²/ (2 * acceleration)
 155
 156         */
 157
 158         // taking the concerned acc. value
 159         if (speed >=0) // why not position ?
 160                 var_2nd_ord = q->var_2nd_ord_pos;
 161         else
 162                 var_2nd_ord = q->var_2nd_ord_neg;
 163
 164         // anticipation, proportionnal to speed. Gain_anticipation is a fixed point value, with 8 bits shift
 165         position_pivot = (ABS(speed) * q->gain_anticipation) >>8 ;
 166
 167         // if necessary, compensation of the output units, when using a sampler divisor
 168         if(q->divisor != 1) {
 169                 var_2nd_ord    *= q-> divisor;
 170                 position_pivot /= q-> divisor;
 171         }
 172
 173         // pivot calculation itself
 174         position_pivot += speed*speed /(2*var_2nd_ord);
 175
 176         // taking the right sign
 177         if(speed >=0)
 178                 position_pivot =  - position_pivot;
 179
 180         // mem only for debug
 181         q-> pivot = position_pivot;
 182
 183         /** this is the heart of the trajectory generation.
 184             Pretty simple but indeed unstable,
 185             because of this corresponds to an infinite gain, in the following equation :
 186             acceleration_consign = ( position_pivot - in_position ) * gain
 187
 188             In fact this unstability is erased by the fact that the acc value is nearly always limited
 189         */
 190         if(position_pivot >= in_position)
 191                 acceleration_consign = q->var_2nd_ord_pos;
 192         else
 193                 acceleration_consign = -q->var_2nd_ord_neg;
 194
 195
 196
 197         /** integration and limitation of the acceleration to obtain a speed consign */
 198         speed_consign = q->previous_out_speed + acceleration_consign;
 199
 200         if (speed_consign >=0) {
 201                 if(q->var_1st_ord_pos)
 202                         MAX(speed_consign , q->var_1st_ord_pos);
 203         }
 204         else {
 205                 if(q->var_1st_ord_neg)
 206                         MIN(speed_consign , -q->var_1st_ord_neg);
 207         }
 208
 209
 210         /** creation of an end arrival window. This is done to stop the oscillations when the goal is achieved. */
 211         if(ABS( in_position ) < q->goal_window)
 212                 speed_consign=0;
 213
 214         /** refresh the memories */
 215         q->previous_in_position = in_position;
 216         q->previous_out_speed = speed_consign;
 217
 218
 219         return speed_consign ;
 220 }