Contiki 2.6
|
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