Contiki 2.6

stepper-move.c

00001 #include <stdio.h>
00002 #include <stepper-interrupt.h>
00003 #include <stepper-move.h>
00004 #include <limits.h>
00005 
00006 #if 0
00007 #define PRINTF(...) printf(__VA_ARGS__)
00008 #else
00009 #define PRINTF(...) do {} while (0)
00010 #endif
00011 
00012 static unsigned int
00013 isqrt(unsigned long x)
00014 {
00015   unsigned int r;
00016   unsigned int b2 = 0x40000000;
00017   unsigned int b = 0x8000;
00018   while(x < b2) {
00019     b2 >>= 2;
00020     b >>= 1;
00021   }
00022   if (b == 0) return 0;
00023   r = b;
00024   b >>= 1;
00025   while(b > 0) {
00026     r += b;
00027     unsigned int t = r*r;
00028     if (t > x) {
00029       r -= b;
00030     }
00031     b >>=1;
00032   }
00033   return r;
00034 }
00035 
00036 #define ACC_FIRST_UP 0
00037 #define ACC_K1_UP 1
00038 #define ACC_LAST_UP 2
00039 #define ACC_TOP 3
00040 #define ACC_FIRST_DOWN 4
00041 #define ACC_K1_DOWN 5
00042 #define ACC_LAST_DOWN 6
00043 #define ACC_END 7
00044 
00045 typedef struct _AccDiff AccDiff;
00046 struct _AccDiff
00047 {
00048   long diff;
00049   unsigned long pos;
00050 };
00051 
00052 
00053 static inline long
00054 base_acc(unsigned long t,unsigned long n, unsigned long l, unsigned long a_max)
00055 {
00056   long a;
00057   if (t >= n) {
00058     if (t >= n+l) {
00059       a = -a_max;
00060     } else {
00061       a = 0;
00062     }
00063   } else {
00064     a = a_max;
00065   }
00066   return a;
00067 }
00068 
00069 static AccDiff acc[ACC_END+1];
00070 StepperResult
00071 stepper_move(unsigned int stepper_index, unsigned long *periodp,
00072              unsigned long a_max,unsigned long v_max, long s_end)
00073 {
00074   unsigned long start_period = *periodp;
00075   unsigned long s;
00076   unsigned long ds;
00077   unsigned long l;
00078   unsigned long da0;
00079   unsigned long k1 = 0;
00080   unsigned long n = (v_max+a_max-1)/a_max;
00081   unsigned long a_speed_adj = v_max - (n-1)*a_max;
00082   unsigned long s_res;
00083   long d;
00084   if (s_end >= 0) {
00085     s_res = s_end/2;
00086   } else {
00087     s_res = (-s_end)/2;
00088   }
00089   d = s_res - (long)a_max*(n*n-1) - (long)a_speed_adj;
00090 
00091   acc[ACC_END].diff = 0;
00092   acc[ACC_END].pos = UINT_MAX;
00093   if (d < 0) {
00094     l = 0;
00095     n = isqrt(s_res/a_max);
00096     if (n*(unsigned long long)n*a_max < s_res) n++;
00097     a_speed_adj = a_max;
00098     acc[ACC_LAST_UP].diff=0;
00099     acc[ACC_FIRST_DOWN].diff=0;
00100   } else {
00101     l = (d+v_max-1)/v_max;
00102     acc[ACC_LAST_UP].diff= a_speed_adj - a_max;
00103     acc[ACC_FIRST_DOWN].diff= a_max - a_speed_adj;
00104   }
00105   acc[ACC_LAST_UP].pos = n-1;
00106   acc[ACC_FIRST_DOWN].pos = n+l;
00107   
00108   s = a_max*(n*n-1) + a_speed_adj + l * (a_max*(n-1) + a_speed_adj);
00109   ds = s-s_res;
00110 
00111   da0 = ds/(2*n+l-1);
00112   acc[ACC_FIRST_UP].diff = -da0;
00113   acc[ACC_LAST_DOWN].diff = da0;
00114   acc[ACC_FIRST_UP].pos = 0;
00115   acc[ACC_LAST_DOWN].pos = 2*n+l-1;
00116   ds -= da0*(2*n+l-1);
00117   
00118   acc[ACC_K1_UP].diff = 0;
00119   acc[ACC_K1_DOWN].diff = 0;
00120   acc[ACC_K1_UP].pos = 0;
00121   acc[ACC_K1_DOWN].pos = 2*n+l-1;
00122 
00123   acc[ACC_TOP].diff = 0;
00124   acc[ACC_TOP].pos = n;
00125   
00126   if (ds > 0) {
00127     k1 = (2*n+l -ds)/2;
00128     if (k1 < n) {
00129 
00130       acc[ACC_K1_UP].diff = -1;
00131       acc[ACC_K1_DOWN].diff = 1;
00132       acc[ACC_K1_UP].pos = k1;
00133       acc[ACC_K1_DOWN].pos = 2*n+l-1 - k1;
00134       ds -= (2*(n-k1)+l-1);
00135     }
00136     if (ds > 0) {
00137       acc[ACC_LAST_UP].diff--;
00138       acc[ACC_TOP].diff = 1;
00139       acc[ACC_TOP].pos = n+ds-1;
00140     }
00141   }
00142 #if 0
00143   {
00144     unsigned int k;
00145     PRINTF("n=%ld l=%ld a_max=%ld v_max=%ld s_res=%ld\n",
00146            n,l ,a_max, v_max, s_res);
00147     for (k = 0; k < 7; k++) {
00148       PRINTF("  %ld@%ld", acc[k].diff, acc[k].pos);
00149     }
00150     PRINTF("\n");
00151   }
00152 #endif
00153   {
00154     StepperResult res;
00155     unsigned int k;
00156     unsigned long t = 0;
00157     long da = 0;
00158     long a_prev = ULONG_MAX;
00159     for (k = 0; k < ACC_END; k++) {
00160       long a;
00161       da += acc[k].diff;
00162       if (acc[k].pos != acc[k+1].pos) { /* Next position is different */
00163         if (t != acc[k].pos) {
00164           a = base_acc(t,n,l,a_max);
00165           if (s_end < 0) a = -a;
00166           if (a_prev != a) {
00167             res = stepper_add_acc(stepper_index, t+start_period, a);
00168             if (res != STEPPER_OK) return res;
00169             PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period);
00170             a_prev = a;
00171           }
00172           t = acc[k].pos;
00173         }
00174         a = da + base_acc(t,n,l,a_max);
00175         if (s_end < 0) a = -a;
00176         if (a_prev != a) {
00177           res = stepper_add_acc(stepper_index, t+start_period, a);
00178           if (res != STEPPER_OK) return res;
00179           PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period);
00180           a_prev = a;
00181         }
00182         t++;
00183         da = 0;
00184       }
00185     }
00186     res = stepper_add_acc(stepper_index, t+start_period, 0);
00187     PRINTF("%d: %d@%ld\n", stepper_index, 0, t+start_period);
00188     if (res != STEPPER_OK) return res;
00189     *periodp += t;
00190   }
00191   return STEPPER_OK;
00192 }
00193