Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 353 lines (281 sloc) 7.928 kb
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
1 #include "dda.h"
2
3 #include <string.h>
4
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
5 #include "pinout.h"
90ea291 Michael Moon time to save again, looking good so far
authored
6 #include "timer.h"
7
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
8 extern struct {
9 volatile int32_t X;
10 volatile int32_t Y;
11 volatile int32_t Z;
12 volatile int32_t E;
13 volatile int32_t F;
14 } current_position;
15
411ada4 Michael Moon this one actually compiles
authored
16 /*
17 move queue
18 */
19
90ea291 Michael Moon time to save again, looking good so far
authored
20 uint8_t mb_head = 0;
21 uint8_t mb_tail = 0;
22 DDA movebuffer[MOVEBUFFER_SIZE];
23
24 uint8_t queue_full() {
25 if (mb_tail == 0)
26 return mb_head == (MOVEBUFFER_SIZE - 1);
27 else
28 return mb_head == (mb_tail - 1);
29 }
30
31 void enqueue(TARGET *t) {
32 while (queue_full())
33 delay(WAITING_DELAY);
34
35 uint8_t h = mb_head;
36 h++;
37 if (h == MOVEBUFFER_SIZE)
38 h = 0;
39 mb_head = h;
40 dda_create(t, &movebuffer[h]);
41 }
42
43 void next_move() {
44 if ((mb_tail == mb_head) && (!movebuffer[mb_tail].live)) {
45 // queue is empty
46 disable_steppers();
47 setTimer(DEFAULT_TICK);
48 }
49 else {
50 uint8_t t = mb_tail;
51 t++;
52 if (t == MOVEBUFFER_SIZE)
53 t = 0;
54 mb_tail = t;
55 dda_start(&movebuffer[t]);
56 }
57 }
58
411ada4 Michael Moon this one actually compiles
authored
59 /*
60 utility functions
61 */
62
90ea291 Michael Moon time to save again, looking good so far
authored
63 // courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
64 uint32_t approx_distance( int32_t dx, int32_t dy )
65 {
90ea291 Michael Moon time to save again, looking good so far
authored
66 uint32_t min, max, approx;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
67
68 if ( dx < 0 ) dx = -dx;
69 if ( dy < 0 ) dy = -dy;
70
71 if ( dx < dy )
72 {
73 min = dx;
74 max = dy;
75 } else {
76 min = dy;
77 max = dx;
78 }
79
90ea291 Michael Moon time to save again, looking good so far
authored
80 approx = ( max * 1007 ) + ( min * 441 );
81 if ( max < ( min << 4 ))
82 approx -= ( max * 40 );
83
84 // add 512 for proper rounding
85 return (( approx + 512 ) >> 10 );
86 }
87
88 // courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance
89 uint32_t approx_distance_3( int32_t dx, int32_t dy, int32_t dz )
90 {
91 uint32_t min, med, max, approx;
92
93 if ( dx < 0 ) dx = -dx;
94 if ( dy < 0 ) dy = -dy;
95 if ( dz < 0 ) dz = -dz;
96
97 if ( dx < dy )
98 {
99 min = dy;
100 med = dx;
101 } else {
102 min = dx;
103 med = dy;
104 }
105
106 if ( dz < (int32_t)min )
107 {
108 max = med;
109 med = min;
110 min = dz;
111 } else if ( dz < (int32_t)med ) {
112 max = med;
113 med = dz;
114 } else {
115 max = dz;
116 }
117
118 approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 );
119 if ( max < ( med << 1 )) approx -= ( max * 294 );
120 if ( max < ( min << 2 )) approx -= ( max * 113 );
121 if ( med < ( min << 2 )) approx -= ( med * 40 );
122
123 // add 512 for proper rounding
124 return (( approx + 512 ) >> 10 );
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
125 }
126
127 /*
128 CREATE
129 */
130
90ea291 Michael Moon time to save again, looking good so far
authored
131 void dda_create(TARGET *target, DDA *dda) {
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
132 static TARGET startpoint = { 0, 0, 0, 0, 0 };
133 uint32_t distance;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
134
135 // we start at the previous endpoint
411ada4 Michael Moon this one actually compiles
authored
136 // memcpy(&dda->currentpoint, &startpoint, sizeof(TARGET));
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
137 // we end at the passed command's endpoint
90ea291 Michael Moon time to save again, looking good so far
authored
138 memcpy(&dda->endpoint, target, sizeof(TARGET));
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
139
140 dda->x_delta = dda->endpoint.X - startpoint.X;
141 dda->y_delta = dda->endpoint.Y - startpoint.Y;
90ea291 Michael Moon time to save again, looking good so far
authored
142 dda->z_delta = dda->endpoint.Z - startpoint.Z;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
143 // always relative
144 dda->e_delta = dda->endpoint.E;
145 // always absolute
146 dda->f_delta = dda->endpoint.F - startpoint.F;
147
90ea291 Michael Moon time to save again, looking good so far
authored
148 // since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation.
149 if (dda->z_delta == 0)
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
150 distance = approx_distance(dda->x_delta, dda->y_delta);
90ea291 Michael Moon time to save again, looking good so far
authored
151 else if (dda->x_delta == 0 && dda->y_delta == 0)
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
152 distance = dda->z_delta;
90ea291 Michael Moon time to save again, looking good so far
authored
153 else
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
154 distance = approx_distance_3(dda->x_delta, dda->y_delta, dda->z_delta);
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
155
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
156 if (distance < 2)
157 distance = dda->e_delta;
158 if (distance < 2)
159 distance = dda->f_delta;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
160
161 dda->total_steps = dda->x_delta;
162 if (dda->y_delta > dda->total_steps)
163 dda->total_steps = dda->y_delta;
90ea291 Michael Moon time to save again, looking good so far
authored
164 if (dda->z_delta > dda->total_steps)
165 dda->total_steps = dda->z_delta;
166
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
167 if (dda->e_delta > dda->total_steps)
168 dda->total_steps = dda->e_delta;
169 if (dda->f_delta > dda->total_steps)
170 dda->total_steps = dda->f_delta;
171
172 if (dda->total_steps == 0)
173 dda->nullmove = 1;
174
175 if (dda->f_delta > dda->total_steps) {
176 dda->f_scale = dda->f_delta / dda->total_steps;
177 if (dda->f_scale > 3) {
178 dda->f_delta /= dda->f_scale;
179 }
180 else {
181 dda->f_scale = 1;
182 dda->total_steps = dda->f_delta;
183 }
184 }
185
186 dda->x_direction = (dda->endpoint.X > startpoint.X)?1:0;
187 dda->y_direction = (dda->endpoint.Y > startpoint.Y)?1:0;
90ea291 Michael Moon time to save again, looking good so far
authored
188 dda->z_direction = (dda->endpoint.Z > startpoint.Z)?1:0;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
189 dda->e_direction = (dda->endpoint.E > startpoint.E)?1:0;
190 dda->f_direction = (dda->endpoint.F > startpoint.F)?1:0;
191
90ea291 Michael Moon time to save again, looking good so far
authored
192 dda->x_counter = dda->y_counter = dda->z_counter = dda->e_counter = dda->f_counter
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
193 = -(dda->total_steps >> 1);
194
90ea291 Michael Moon time to save again, looking good so far
authored
195 // pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context
196 // mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min
411ada4 Michael Moon this one actually compiles
authored
197 // so in the interrupt we must simply calculate
198 // mm.us per step.min / mm per min (F) = us per step
496d58f Michael Moon time to save again, preliminary PID/PWM support for extruder barrel heat...
authored
199 dda->move_duration = distance * 60000000 / dda->total_steps;
90ea291 Michael Moon time to save again, looking good so far
authored
200
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
201 // next dda starts where we finish
202 memcpy(&startpoint, &dda->endpoint, sizeof(TARGET));
90ea291 Michael Moon time to save again, looking good so far
authored
203
411ada4 Michael Moon this one actually compiles
authored
204 // make sure we're not running
90ea291 Michael Moon time to save again, looking good so far
authored
205 dda->live = 0;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
206 }
207
208 /*
209 START
210 */
211
212 void dda_start(DDA *dda) {
90ea291 Michael Moon time to save again, looking good so far
authored
213 // called from interrupt context: keep it simple!
214 if (dda->nullmove)
215 return;
216
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
217 x_direction(dda->x_direction);
218 y_direction(dda->y_direction);
219 z_direction(dda->z_direction);
220 e_direction(dda->e_direction);
90ea291 Michael Moon time to save again, looking good so far
authored
221
222 enable_steppers();
411ada4 Michael Moon this one actually compiles
authored
223
90ea291 Michael Moon time to save again, looking good so far
authored
224 dda->live = 1;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
225 }
226
227 /*
228 CAN STEP
229 */
230
231 uint8_t can_step(uint8_t min, uint8_t max, int32_t current, int32_t target, uint8_t dir) {
232 if (target == current)
233 return 0;
234
235 if (min && !dir)
236 return 0;
237
238 if (max && dir)
239 return 0;
240
241 return 255;
242 }
243
244 /*
245 STEP
246 */
247
248 void dda_step(DDA *dda) {
249 uint8_t step_option = 0;
250 #define X_CAN_STEP 1
251 #define Y_CAN_STEP 2
252 #define Z_CAN_STEP 4
253 #define E_CAN_STEP 8
254 #define F_CAN_STEP 16
255 #define REAL_MOVE 32
256
257 do {
258 step_option |= can_step(x_min(), x_max(), current_position.X, dda->endpoint.X, dda->x_direction) & X_CAN_STEP;
259 step_option |= can_step(y_min(), y_max(), current_position.Y, dda->endpoint.Y, dda->y_direction) & Y_CAN_STEP;
260 step_option |= can_step(z_min(), z_max(), current_position.Z, dda->endpoint.Z, dda->z_direction) & Z_CAN_STEP;
411ada4 Michael Moon this one actually compiles
authored
261 step_option |= can_step(0 , 0 , current_position.E, dda->endpoint.E, dda->e_direction) & E_CAN_STEP;
262 step_option |= can_step(0 , 0 , current_position.F, dda->endpoint.F, dda->f_direction) & F_CAN_STEP;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
263
264 if (step_option & X_CAN_STEP) {
90ea291 Michael Moon time to save again, looking good so far
authored
265 dda->x_counter -= dda->x_delta;
266 if (dda->x_counter < 0) {
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
267 step_option |= REAL_MOVE;
268
90ea291 Michael Moon time to save again, looking good so far
authored
269 x_step();
270
271 dda->x_counter += dda->total_steps;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
272
273 if (dda->x_direction)
274 current_position.X++;
275 else
276 current_position.X--;
277 }
278 }
279
280 if (step_option & Y_CAN_STEP) {
90ea291 Michael Moon time to save again, looking good so far
authored
281 dda->y_counter -= dda->y_delta;
282 if (dda->y_counter < 0) {
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
283 step_option |= REAL_MOVE;
284
90ea291 Michael Moon time to save again, looking good so far
authored
285 y_step();
286
287 dda->y_counter += dda->total_steps;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
288
289 if (dda->y_direction)
290 current_position.Y++;
291 else
292 current_position.Y--;
293 }
294 }
295
296 if (step_option & Z_CAN_STEP) {
90ea291 Michael Moon time to save again, looking good so far
authored
297 dda->z_counter -= dda->z_delta;
298 if (dda->z_counter < 0) {
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
299 step_option |= REAL_MOVE;
300
90ea291 Michael Moon time to save again, looking good so far
authored
301 z_step();
302
303 dda->z_counter += dda->total_steps;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
304
305 if (dda->z_direction)
306 current_position.Z++;
307 else
308 current_position.Z--;
309 }
310 }
311
312 if (step_option & E_CAN_STEP) {
90ea291 Michael Moon time to save again, looking good so far
authored
313 dda->e_counter -= dda->e_delta;
314 if (dda->e_counter < 0) {
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
315 step_option |= REAL_MOVE;
316
90ea291 Michael Moon time to save again, looking good so far
authored
317 e_step();
318
319 dda->e_counter += dda->total_steps;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
320
321 if (dda->e_direction)
322 current_position.E++;
323 else
324 current_position.E--;
325 }
326 }
327
328 if (step_option & F_CAN_STEP) {
90ea291 Michael Moon time to save again, looking good so far
authored
329 dda->f_counter -= dda->f_delta;
330 if (dda->f_counter < 0) {
331
332 dda->f_counter += dda->total_steps;
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
333
334 if (dda->f_direction)
335 current_position.F += dda->f_scale;
336 else
337 current_position.F -= dda->f_scale;
338 }
339 }
90ea291 Michael Moon time to save again, looking good so far
authored
340 } while ( ((step_option & REAL_MOVE ) == 0) &&
341 ((step_option & F_CAN_STEP) != 0) );
342
411ada4 Michael Moon this one actually compiles
authored
343 // turn off step outputs, hopefully they've been on long enough by now to register with the drivers
90ea291 Michael Moon time to save again, looking good so far
authored
344 unstep();
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
345
411ada4 Michael Moon this one actually compiles
authored
346 // we have stepped and now need to wait
347 if (step_option & REAL_MOVE)
90ea291 Michael Moon time to save again, looking good so far
authored
348 setTimer(dda->move_duration / current_position.F);
349
411ada4 Michael Moon this one actually compiles
authored
350 // if we could step, we're still running
90ea291 Michael Moon time to save again, looking good so far
authored
351 dda->live = (step_option & (X_CAN_STEP | Y_CAN_STEP | Z_CAN_STEP | E_CAN_STEP | F_CAN_STEP));
1e6c740 Michael Moon tons of changes, implementing 4D dda
authored
352 }
Something went wrong with that request. Please try again.