quadramp issues -- not really fixed
[aversive.git] / modules / devices / control_system / filters / quadramp / quadramp.c
index 847d01a..26d80c5 100644 (file)
@@ -1,7 +1,6 @@
-
-/*  
+/*
  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -27,9 +26,7 @@
 #include <aversive.h>
 #include <quadramp.h>
 
 #include <aversive.h>
 #include <quadramp.h>
 
-#define NEXT(n, i)  (((n) + (i)/(n)) >> 1)
-
-void quadramp_init(struct quadramp_filter * q)
+void quadramp_init(struct quadramp_filter *q)
 {
        uint8_t flags;
        IRQ_LOCK(flags);
 {
        uint8_t flags;
        IRQ_LOCK(flags);
@@ -38,16 +35,16 @@ void quadramp_init(struct quadramp_filter * q)
 }
 
 
 }
 
 
-void quadramp_reset(struct quadramp_filter * q)
+void quadramp_reset(struct quadramp_filter *q)
 {
        q->previous_var = 0;
        q->previous_out = 0;
        q->previous_in = 0;
 }
 
 {
        q->previous_var = 0;
        q->previous_out = 0;
        q->previous_in = 0;
 }
 
-void quadramp_set_2nd_order_vars(struct quadramp_filter * q, 
-                                uint32_t var_2nd_ord_pos, 
-                                uint32_t var_2nd_ord_neg)
+void quadramp_set_2nd_order_vars(struct quadramp_filter *q,
+                                double var_2nd_ord_pos,
+                                double var_2nd_ord_neg)
 {
        uint8_t flags;
        IRQ_LOCK(flags);
 {
        uint8_t flags;
        IRQ_LOCK(flags);
@@ -56,9 +53,9 @@ void quadramp_set_2nd_order_vars(struct quadramp_filter * q,
        IRQ_UNLOCK(flags);
 }
 
        IRQ_UNLOCK(flags);
 }
 
-void quadramp_set_1st_order_vars(struct quadramp_filter * q, 
-                                uint32_t var_1st_ord_pos, 
-                                uint32_t var_1st_ord_neg)
+void quadramp_set_1st_order_vars(struct quadramp_filter *q,
+                                double var_1st_ord_pos,
+                                double var_1st_ord_neg)
 {
        uint8_t flags;
        IRQ_LOCK(flags);
 {
        uint8_t flags;
        IRQ_LOCK(flags);
@@ -70,118 +67,123 @@ void quadramp_set_1st_order_vars(struct quadramp_filter * q,
 
 uint8_t quadramp_is_finished(struct quadramp_filter *q)
 {
 
 uint8_t quadramp_is_finished(struct quadramp_filter *q)
 {
-       return (q->previous_out == q->previous_in &&
+       return ((int32_t)q->previous_out == q->previous_in &&
                q->previous_var == 0);
 }
 
 /**
  * Process the ramp
                q->previous_var == 0);
 }
 
 /**
  * Process the ramp
- * 
+ *
  * \param data should be a (struct quadramp_filter *) pointer
  * \param in is the input of the filter
  * \param data should be a (struct quadramp_filter *) pointer
  * \param in is the input of the filter
- * 
+ *
  */
 int32_t quadramp_do_filter(void * data, int32_t in)
 {
        struct quadramp_filter * q = data;
  */
 int32_t quadramp_do_filter(void * data, int32_t in)
 {
        struct quadramp_filter * q = data;
-       int32_t d ;
+       int32_t d; /* remaining distance */
        int32_t pos_target;
        int32_t pos_target;
-       int32_t var_1st_ord_pos = 0;
-       int32_t var_1st_ord_neg = 0;
-       int32_t var_2nd_ord_pos = 0;
-       int32_t var_2nd_ord_neg = 0;
-       int32_t previous_var, previous_out ;
-
-       if ( q->var_1st_ord_pos )
-               var_1st_ord_pos = q->var_1st_ord_pos ;  
-
-       if ( q->var_1st_ord_neg )
-               var_1st_ord_neg = -q->var_1st_ord_neg ;
-
-       if ( q->var_2nd_ord_pos )
-               var_2nd_ord_pos = q->var_2nd_ord_pos ;  
-
-       if ( q->var_2nd_ord_neg )
-               var_2nd_ord_neg = -q->var_2nd_ord_neg ;
+       double var_1st_ord_pos = q->var_1st_ord_pos;
+       double var_1st_ord_neg = -q->var_1st_ord_neg;
+       double var_2nd_ord_pos = q->var_2nd_ord_pos;
+       double var_2nd_ord_neg = -q->var_2nd_ord_neg;
+       double previous_var, d_float;
+       double previous_out;
 
        previous_var = q->previous_var;
        previous_out = q->previous_out;
 
 
        previous_var = q->previous_var;
        previous_out = q->previous_out;
 
-       d = in - previous_out ;
+       d_float = (double)in - previous_out ;
+
+       /* d is very small, we can jump to dest */
+       if (fabs(d_float) < 2.) {
+               q->previous_var = 0;
+               q->previous_out = in;
+               q->previous_in = in;
+               return in;
+       }
+
+       d = (int32_t)d_float;
 
        /* Deceleration ramp */
 
        /* Deceleration ramp */
-       if ( d > 0 && var_2nd_ord_neg) {
-               int32_t ramp_pos;
+       if (d > 0 && var_2nd_ord_neg) {
+               double ramp_pos;
                /* var_2nd_ord_neg < 0 */
                /* var_2nd_ord_neg < 0 */
-               /* real EQ : sqrt( var_2nd_ord_neg^2/4 - 2.d.var_2nd_ord_neg ) + var_2nd_ord_neg/2 */
-               ramp_pos = sqrt( (var_2nd_ord_neg*var_2nd_ord_neg)/4 - 2*d*var_2nd_ord_neg ) + var_2nd_ord_neg/2;
+               ramp_pos = sqrt((var_2nd_ord_neg*var_2nd_ord_neg)/4 -
+                               2*d_float*var_2nd_ord_neg) +
+                       var_2nd_ord_neg/2;
 
 
-               if(ramp_pos < var_1st_ord_pos)
-                       var_1st_ord_pos = ramp_pos ;
+               if (ramp_pos < var_1st_ord_pos)
+                       var_1st_ord_pos = ramp_pos;
        }
 
        else if (d < 0 && var_2nd_ord_pos) {
        }
 
        else if (d < 0 && var_2nd_ord_pos) {
-               int32_t ramp_neg;
-    
+               double ramp_neg;
+
                /* var_2nd_ord_pos > 0 */
                /* var_2nd_ord_pos > 0 */
-               /* real EQ : sqrt( var_2nd_ord_pos^2/4 - 2.d.var_2nd_ord_pos ) - var_2nd_ord_pos/2 */
-               ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 - 2*d*var_2nd_ord_pos ) - var_2nd_ord_pos/2;
-       
+               ramp_neg = -sqrt( (var_2nd_ord_pos*var_2nd_ord_pos)/4 -
+                                 2*d_float*var_2nd_ord_pos ) -
+                       var_2nd_ord_pos/2;
+
                /* ramp_neg < 0 */
                /* ramp_neg < 0 */
-               if(ramp_neg > var_1st_ord_neg)
+               if (ramp_neg > var_1st_ord_neg)
                        var_1st_ord_neg = ramp_neg ;
        }
                        var_1st_ord_neg = ramp_neg ;
        }
-    
+
        /* try to set the speed : can we reach the speed with our acceleration ? */
        /* si on va moins vite que la Vmax */
        /* try to set the speed : can we reach the speed with our acceleration ? */
        /* si on va moins vite que la Vmax */
-       if ( previous_var < var_1st_ord_pos )  {
+       if (previous_var < var_1st_ord_pos)  {
                /* acceleration would be to high, we reduce the speed */
                /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
                 * on sature Vmax a Vcourante + acceleration */
                /* acceleration would be to high, we reduce the speed */
                /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmax,
                 * on sature Vmax a Vcourante + acceleration */
-               if (var_2nd_ord_pos && ( var_1st_ord_pos - previous_var > var_2nd_ord_pos) )
-                       var_1st_ord_pos = previous_var + var_2nd_ord_pos ;
+               if (var_2nd_ord_pos &&
+                   (var_1st_ord_pos - previous_var) > var_2nd_ord_pos)
+                       var_1st_ord_pos = previous_var + var_2nd_ord_pos;
        }
        /* si on va plus vite que Vmax */
        }
        /* si on va plus vite que Vmax */
-       else if ( previous_var > var_1st_ord_pos )  { 
+       else if (previous_var > var_1st_ord_pos)  {
                /* deceleration would be to high, we increase the speed */
                /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax,
                 * on sature Vmax a Vcourante + deceleration */
                /* deceleration would be to high, we increase the speed */
                /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmax,
                 * on sature Vmax a Vcourante + deceleration */
-               if (var_2nd_ord_neg && ( var_1st_ord_pos - previous_var < var_2nd_ord_neg) )
+               if (var_2nd_ord_neg &&
+                   (var_1st_ord_pos - previous_var) < var_2nd_ord_neg)
                        var_1st_ord_pos = previous_var + var_2nd_ord_neg;
        }
                        var_1st_ord_pos = previous_var + var_2nd_ord_neg;
        }
-  
+
        /* same for the neg */
        /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */
        /* same for the neg */
        /* si on va plus vite que la Vmin (en negatif : en vrai la vitesse absolue est inferieure) */
-       if ( previous_var > var_1st_ord_neg )  {
+       if (previous_var > var_1st_ord_neg)  {
                /* acceleration would be to high, we reduce the speed */
                /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin,
                 * on sature Vmax a Vcourante + deceleration */
                /* acceleration would be to high, we reduce the speed */
                /* si rampe deceleration active ET qu'on ne peut pas atteindre Vmin,
                 * on sature Vmax a Vcourante + deceleration */
-               if (var_2nd_ord_neg && ( var_1st_ord_neg - previous_var < var_2nd_ord_neg) )
-                       var_1st_ord_neg = previous_var + var_2nd_ord_neg ;
+               if (var_2nd_ord_neg &&
+                   (var_1st_ord_neg - previous_var) < var_2nd_ord_neg)
+                       var_1st_ord_neg = previous_var + var_2nd_ord_neg;
        }
        /* si on va moins vite que Vmin (mais vitesse absolue superieure) */
        }
        /* si on va moins vite que Vmin (mais vitesse absolue superieure) */
-       else if ( previous_var < var_1st_ord_neg )  {
+       else if (previous_var < var_1st_ord_neg)  {
                /* deceleration would be to high, we increase the speed */
                /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
                 * on sature Vmax a Vcourante + deceleration */
                /* deceleration would be to high, we increase the speed */
                /* si rampe acceleration active ET qu'on ne peut pas atteindre Vmin,
                 * on sature Vmax a Vcourante + deceleration */
-               if (var_2nd_ord_pos && (var_1st_ord_neg - previous_var > var_2nd_ord_pos) )
+               if (var_2nd_ord_pos &&
+                   (var_1st_ord_neg - previous_var) > var_2nd_ord_pos)
                        var_1st_ord_neg = previous_var + var_2nd_ord_pos;
        }
 
        /*
         * Position consign : can we reach the position with our speed ?
         */
                        var_1st_ord_neg = previous_var + var_2nd_ord_pos;
        }
 
        /*
         * Position consign : can we reach the position with our speed ?
         */
-       if ( /* var_1st_ord_pos &&  */d > var_1st_ord_pos ) {
+       if (d_float > var_1st_ord_pos) {
                pos_target = previous_out + var_1st_ord_pos ;
                previous_var = var_1st_ord_pos ;
        }
                pos_target = previous_out + var_1st_ord_pos ;
                previous_var = var_1st_ord_pos ;
        }
-       else if ( /* var_1st_ord_neg &&  */d < var_1st_ord_neg ) {
+       else if (d_float < var_1st_ord_neg) {
                pos_target = previous_out + var_1st_ord_neg ;
                previous_var = var_1st_ord_neg ;
        }
        else {
                pos_target = previous_out + var_1st_ord_neg ;
                previous_var = var_1st_ord_neg ;
        }
        else {
-               pos_target = previous_out + d ;
-               previous_var = d ;
+               pos_target = in;
+               previous_var = d_float ;
        }
 
        // update previous_out and previous_var
        }
 
        // update previous_out and previous_var
@@ -189,5 +191,5 @@ int32_t quadramp_do_filter(void * data, int32_t in)
        q->previous_out = pos_target;
        q->previous_in = in;
 
        q->previous_out = pos_target;
        q->previous_in = in;
 
-       return pos_target ;
+       return pos_target;
 }
 }