PyCNC

1
from __future__ import division
2
import logging
3

4
from cnc.config import *
5
from cnc.coordinates import *
6
from cnc.enums import *
7

8
SECONDS_IN_MINUTE = 60.0
9

10

11
class PulseGenerator(object):
12
    """ Stepper motors pulses generator.
13
        It generates time for each pulses for specified path as accelerated
14
        movement for specified velocity, then moves linearly and then braking
15
        with the same acceleration.
16
        Internally this class treat movement as uniform movement and then
17
        translate timings to accelerated movements. To do so, it base on
18
        formulas for distance of uniform movement and accelerated move.
19
            S = V * Ta = a * Tu^2 / 2
20
        where Ta - time for accelerated and Tu for uniform movement.
21
        Velocity will never be more then Vmax - maximum velocity of all axises.
22
        At the point of maximum velocity we change accelerated movement to
23
        uniform, so we can translate time for accelerated movement with this
24
        formula:
25
            Ta(Tu) = a * Tu^2 / Vmax / 2
26
        Now we need just to calculate how much time will accelerate and
27
        brake will take and recalculate time for them. Linear part will be as
28
        is. Since maximum velocity and acceleration is always the same, there
29
        is the ACCELERATION_FACTOR_PER_SEC variable.
30
        In the same way circular or other interpolation can be implemented
31
        based this class.
32
    """
33
    AUTO_VELOCITY_ADJUSTMENT = AUTO_VELOCITY_ADJUSTMENT
34

35
    def __init__(self, delta):
36
        """ Create object. Do not create directly this object, inherit this
37
            class and implement interpolation function and related methods.
38
            All child have to call this method ( super().__init__() ).
39
            :param delta: overall movement delta in mm, uses for debug purpose.
40
        """
41
        self._iteration_x = 0
42
        self._iteration_y = 0
43
        self._iteration_z = 0
44
        self._iteration_e = 0
45
        self._iteration_direction = None
46
        self._acceleration_time_s = 0.0
47
        self._linear_time_s = 0.0
48
        self._2Vmax_per_a = 0.0
49
        self._delta = delta
50

51
    def _adjust_velocity(self, velocity_mm_sec):
52
        """ Automatically decrease velocity to all axises proportionally if
53
        velocity for one or more axises is more then maximum velocity for axis.
54
        :param velocity_mm_sec: input velocity.
55
        :return: adjusted(decreased if needed) velocity.
56
        """
57
        if not self.AUTO_VELOCITY_ADJUSTMENT:
58
            return velocity_mm_sec
59
        k = 1.0
60
        if velocity_mm_sec.x * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_X:
61
            k = min(k, MAX_VELOCITY_MM_PER_MIN_X
62
                    / velocity_mm_sec.x / SECONDS_IN_MINUTE)
63
        if velocity_mm_sec.y * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_Y:
64
            k = min(k, MAX_VELOCITY_MM_PER_MIN_Y
65
                    / velocity_mm_sec.y / SECONDS_IN_MINUTE)
66
        if velocity_mm_sec.z * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_Z:
67
            k = min(k, MAX_VELOCITY_MM_PER_MIN_Z
68
                    / velocity_mm_sec.z / SECONDS_IN_MINUTE)
69
        if velocity_mm_sec.e * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_E:
70
            k = min(k, MAX_VELOCITY_MM_PER_MIN_E
71
                    / velocity_mm_sec.e / SECONDS_IN_MINUTE)
72
        if k != 1.0:
73
            logging.warning("Out of speed, multiply velocity by {}".format(k))
74
        return velocity_mm_sec * k
75

76
    def _get_movement_parameters(self):
77
        """ Get parameters for interpolation. This method have to be
78
            reimplemented in parent classes and should calculate 3 parameters.
79
        :return: Tuple of three values:
80
                acceleration_time_s: time for accelerating and breaking motors
81
                                     during movement
82
                linear_time_s: time for uniform movement, it is total movement
83
                               time minus acceleration and braking time
84
                max_axis_velocity_mm_per_sec: maximum axis velocity of all
85
                                              axises during movement. Even if
86
                                              whole movement is accelerated,
87
                                              this value should be calculated
88
                                              as top velocity.
89
        """
90
        raise NotImplemented
91

92
    def _interpolation_function(self, ix, iy, iz, ie):
93
        """ Get function for interpolation path. This function should returned
94
            values as it is uniform movement. There is only one trick, function
95
            must be expressed in terms of position, i.e. t = S / V for linear,
96
            where S - distance would be increment on motor minimum step.
97
        :param ix: number of pulse for X axis.
98
        :param iy: number of pulse for Y axis.
99
        :param iz: number of pulse for Z axis.
100
        :param ie: number of pulse for E axis.
101
        :return: Two tuples. First is tuple is directions for each axis,
102
                 positive means forward, negative means reverse. Second is
103
                 tuple of times for each axis in us or None if movement for
104
                 axis is finished.
105
        """
106
        raise NotImplemented
107

108
    def __iter__(self):
109
        """ Get iterator.
110
        :return: iterable object.
111
        """
112
        (self._acceleration_time_s, self._linear_time_s,
113
         max_axis_velocity_mm_per_sec) = self._get_movement_parameters()
114
        # helper variable
115
        self._2Vmax_per_a = (2.0 * max_axis_velocity_mm_per_sec.find_max()
116
                             / STEPPER_MAX_ACCELERATION_MM_PER_S2)
117
        self._iteration_x = 0
118
        self._iteration_y = 0
119
        self._iteration_z = 0
120
        self._iteration_e = 0
121
        self._iteration_direction = None
122
        logging.debug(', '.join("%s: %s" % i for i in vars(self).items()))
123
        return self
124

125
    def _to_accelerated_time(self, pt_s):
126
        """ Internal function to translate uniform movement time to time for
127
            accelerated movement.
128
        :param pt_s: pseudo time of uniform movement.
129
        :return: time for each axis or None if movement for axis is finished.
130
        """
131
        # acceleration
132
        # S = Tpseudo * Vmax = a * t^2 / 2
133
        t = math.sqrt(pt_s * self._2Vmax_per_a)
134
        if t <= self._acceleration_time_s:
135
            return t
136

137
        # linear
138
        # pseudo acceleration time Tpseudo = t^2 / ACCELERATION_FACTOR_PER_SEC
139
        t = self._acceleration_time_s + pt_s - (self._acceleration_time_s ** 2
140
                                                / self._2Vmax_per_a)
141
        # pseudo breaking time
142
        bt = t - self._acceleration_time_s - self._linear_time_s
143
        if bt <= 0:
144
            return t
145

146
        # braking
147
        # Vmax * Tpseudo = Vlinear * t - a * t^2 / 2
148
        # V on start braking is Vlinear = Taccel * a = Tbreaking * a
149
        # Vmax * Tpseudo = Tbreaking * a * t - a * t^2 / 2
150
        d = self._acceleration_time_s ** 2 - self._2Vmax_per_a * bt
151
        if d > 0:
152
            d = math.sqrt(d)
153
        else:
154
            d = 0
155
        return 2.0 * self._acceleration_time_s + self._linear_time_s - d
156

157
    def __next__(self):
158
        # for python3
159
        return self.next()
160

161
    def next(self):
162
        """ Iterate pulses.
163
        :return: Tuple of five values:
164
                    - first is boolean value, if it is True, motors direction
165
                        should be changed and next pulse should performed in
166
                        this direction.
167
                    - values for all machine axises. For direction update,
168
                        positive values means forward movement, negative value
169
                        means reverse movement. For normal pulse, values are
170
                        represent time for the next pulse in microseconds.
171
                 This iteration strictly guarantees that next pulses time will
172
                 not be earlier in time then current. If there is no pulses
173
                 left StopIteration will be raised.
174
        """
175
        direction, (tx, ty, tz, te) = \
176
            self._interpolation_function(self._iteration_x, self._iteration_y,
177
                                         self._iteration_z, self._iteration_e)
178
        # check if direction update:
179
        if direction != self._iteration_direction:
180
            self._iteration_direction = direction
181
            dir_x, dir_y, dir_z, dir_e = direction
182
            if STEPPER_INVERTED_X:
183
                dir_x = -dir_x
184
            if STEPPER_INVERTED_Y:
185
                dir_y = -dir_y
186
            if STEPPER_INVERTED_Z:
187
                dir_z = -dir_z
188
            if STEPPER_INVERTED_E:
189
                dir_e = -dir_e
190
            return True, dir_x, dir_y, dir_z, dir_e
191
        # check condition to stop
192
        if tx is None and ty is None and tz is None and te is None:
193
            raise StopIteration
194

195
        # convert to real time
196
        m = None
197
        for i in (tx, ty, tz, te):
198
            if i is not None and (m is None or i < m):
199
                m = i
200
        am = self._to_accelerated_time(m)
201
        # sort pulses in time
202
        if tx is not None:
203
            if tx > m:
204
                tx = None
205
            else:
206
                tx = am
207
                self._iteration_x += 1
208
        if ty is not None:
209
            if ty > m:
210
                ty = None
211
            else:
212
                ty = am
213
                self._iteration_y += 1
214
        if tz is not None:
215
            if tz > m:
216
                tz = None
217
            else:
218
                tz = am
219
                self._iteration_z += 1
220
        if te is not None:
221
            if te > m:
222
                te = None
223
            else:
224
                te = am
225
                self._iteration_e += 1
226

227
        return False, tx, ty, tz, te
228

229
    def total_time_s(self):
230
        """ Get total time for movement.
231
        :return: time in seconds.
232
        """
233
        acceleration_time_s, linear_time_s, _ = self._get_movement_parameters()
234
        return acceleration_time_s * 2.0 + linear_time_s
235

236
    def delta(self):
237
        """ Get overall movement distance.
238
        :return: Movement distance for each axis in millimeters.
239
        """
240
        return self._delta
241

242
    def max_velocity(self):
243
        """ Get max velocity for each axis.
244
        :return: Vector with max velocity(in mm per min) for each axis.
245
        """
246
        _, _, v = self._get_movement_parameters()
247
        return v * SECONDS_IN_MINUTE
248

249

250
class PulseGeneratorLinear(PulseGenerator):
251
    def __init__(self, delta_mm, velocity_mm_per_min):
252
        """ Create pulse generator for linear interpolation.
253
        :param delta_mm: movement distance of each axis.
254
        :param velocity_mm_per_min: desired velocity.
255
        """
256
        super(PulseGeneratorLinear, self).__init__(delta_mm)
257
        distance_mm = abs(delta_mm)  # type: Coordinates
258
        # velocity of each axis
259
        distance_total_mm = distance_mm.length()
260
        self.max_velocity_mm_per_sec = self._adjust_velocity(distance_mm * (
261
            velocity_mm_per_min / SECONDS_IN_MINUTE / distance_total_mm))
262
        # acceleration time
263
        self.acceleration_time_s = (self.max_velocity_mm_per_sec.find_max()
264
                                    / STEPPER_MAX_ACCELERATION_MM_PER_S2)
265
        # check if there is enough space to accelerate and brake, adjust time
266
        # S = a * t^2 / 2
267
        if STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s ** 2 \
268
                > distance_total_mm:
269
            self.acceleration_time_s = \
270
                math.sqrt(distance_total_mm
271
                          / STEPPER_MAX_ACCELERATION_MM_PER_S2)
272
            self.linear_time_s = 0.0
273
            # V = a * t -> V = 2 * S / t, take half of total distance for
274
            # acceleration and braking
275
            self.max_velocity_mm_per_sec = (distance_mm
276
                                            / self.acceleration_time_s)
277
        else:
278
            # calculate linear time
279
            linear_distance_mm = distance_total_mm \
280
                                 - self.acceleration_time_s ** 2 \
281
                                 * STEPPER_MAX_ACCELERATION_MM_PER_S2
282
            self.linear_time_s = (linear_distance_mm
283
                                  / self.max_velocity_mm_per_sec.length())
284
        self._total_pulses_x = round(distance_mm.x * STEPPER_PULSES_PER_MM_X)
285
        self._total_pulses_y = round(distance_mm.y * STEPPER_PULSES_PER_MM_Y)
286
        self._total_pulses_z = round(distance_mm.z * STEPPER_PULSES_PER_MM_Z)
287
        self._total_pulses_e = round(distance_mm.e * STEPPER_PULSES_PER_MM_E)
288
        self._direction = (math.copysign(1, delta_mm.x),
289
                           math.copysign(1, delta_mm.y),
290
                           math.copysign(1, delta_mm.z),
291
                           math.copysign(1, delta_mm.e))
292

293
    def _get_movement_parameters(self):
294
        """ Return movement parameters, see super class for details.
295
        """
296
        return (self.acceleration_time_s,
297
                self.linear_time_s,
298
                self.max_velocity_mm_per_sec)
299

300
    @staticmethod
301
    def __linear(i, pulses_per_mm, total_pulses, velocity_mm_per_sec):
302
        """ Helper function for linear movement.
303
        """
304
        # check if need to calculate for this axis
305
        if total_pulses == 0.0 or i >= total_pulses:
306
            return None
307
        # Linear movement, S = V * t -> t = S / V
308
        return i / pulses_per_mm / velocity_mm_per_sec
309

310
    def _interpolation_function(self, ix, iy, iz, ie):
311
        """ Calculate interpolation values for linear movement, see super class
312
            for details.
313
        """
314
        t_x = self.__linear(ix, STEPPER_PULSES_PER_MM_X, self._total_pulses_x,
315
                            self.max_velocity_mm_per_sec.x)
316
        t_y = self.__linear(iy, STEPPER_PULSES_PER_MM_Y, self._total_pulses_y,
317
                            self.max_velocity_mm_per_sec.y)
318
        t_z = self.__linear(iz, STEPPER_PULSES_PER_MM_Z, self._total_pulses_z,
319
                            self.max_velocity_mm_per_sec.z)
320
        t_e = self.__linear(ie, STEPPER_PULSES_PER_MM_E, self._total_pulses_e,
321
                            self.max_velocity_mm_per_sec.e)
322
        return self._direction, (t_x, t_y, t_z, t_e)
323

324

325
class PulseGeneratorCircular(PulseGenerator):
326
    def __init__(self, delta, radius, plane, direction, velocity):
327
        """ Create pulse generator for circular interpolation.
328
            Position calculates based on formulas:
329
            R^2 = x^2 + y^2
330
            x = R * sin(phi)
331
            y = R * cos(phi)
332
            phi = omega * t,   2 * pi / omega = 2 * pi * R / V
333
            phi = V * t / R
334
            omega is angular_velocity.
335
            so t = V / R * phi
336
            phi can be calculated based on steps position.
337
            Each axis can calculate circle phi base on iteration number, the
338
            only one difference, that there is four quarters of circle and
339
            signs for movement and solving expressions are different. So
340
            we use additional variables to control it.
341
            :param delta: finish position delta from the beginning, must be on
342
                          circle on specified plane. Zero means full circle.
343
            :param radius: vector to center of circle.
344
            :param plane: plane to interpolate.
345
            :param direction: clockwise or counterclockwise.
346
            :param velocity: velocity in mm per min.
347
        """
348
        super(PulseGeneratorCircular, self).__init__(delta)
349
        self._plane = plane
350
        self._direction = direction
351
        velocity = velocity / SECONDS_IN_MINUTE
352
        # Get circle start point and end point.
353
        if self._plane == PLANE_XY:
354
            sa = -radius.x
355
            sb = -radius.y
356
            ea = sa + delta.x
357
            eb = sb + delta.y
358
            apm = STEPPER_PULSES_PER_MM_X
359
            bpm = STEPPER_PULSES_PER_MM_Y
360
        elif self._plane == PLANE_YZ:
361
            sa = -radius.y
362
            sb = -radius.z
363
            ea = sa + delta.y
364
            eb = sb + delta.z
365
            apm = STEPPER_PULSES_PER_MM_Y
366
            bpm = STEPPER_PULSES_PER_MM_Z
367
        elif self._plane == PLANE_ZX:
368
            sa = -radius.z
369
            sb = -radius.x
370
            ea = sa + delta.z
371
            eb = sb + delta.x
372
            apm = STEPPER_PULSES_PER_MM_Z
373
            bpm = STEPPER_PULSES_PER_MM_X
374
        else:
375
            raise ValueError("Unknown plane")
376
        # adjust radius to fit into axises step.
377
        radius = (round(math.sqrt(sa * sa + sb * sb) * min(apm, bpm))
378
                  / min(apm, bpm))
379
        radius_a = (round(math.sqrt(sa * sa + sb * sb) * apm) / apm)
380
        radius_b = (round(math.sqrt(sa * sa + sb * sb) * bpm) / bpm)
381
        self._radius_a2 = radius_a * radius_a
382
        self._radius_b2 = radius_b * radius_b
383
        self._radius_a_pulses = int(radius * apm)
384
        self._radius_b_pulses = int(radius * bpm)
385
        self._start_a_pulses = int(sa * apm)
386
        self._start_b_pulses = int(sb * bpm)
387
        assert (round(math.sqrt(ea * ea + eb * eb) * min(apm, bpm))
388
                / min(apm, bpm) == radius), "Wrong end point"
389

390
        # Calculate angles and directions.
391
        start_angle = self.__angle(sa, sb)
392
        end_angle = self.__angle(ea, eb)
393
        delta_angle = end_angle - start_angle
394
        if delta_angle < 0 or (delta_angle == 0 and direction == CW):
395
            delta_angle += 2 * math.pi
396
        if direction == CCW:
397
            delta_angle -= 2 * math.pi
398
        if direction == CW:
399
            if start_angle >= math.pi:
400
                self._dir_b = 1
401
            else:
402
                self._dir_b = -1
403
            if math.pi / 2 <= start_angle < 3 * math.pi / 2:
404
                self._dir_a = -1
405
            else:
406
                self._dir_a = 1
407
        elif direction == CCW:
408
            if 0.0 < start_angle <= math.pi:
409
                self._dir_b = 1
410
            else:
411
                self._dir_b = -1
412
            if start_angle <= math.pi / 2 or start_angle > 3 * math.pi / 2:
413
                self._dir_a = -1
414
            else:
415
                self._dir_a = 1
416
        self._side_a = (self._start_b_pulses < 0
417
                        or (self._start_b_pulses == 0 and self._dir_b < 0))
418
        self._side_b = (self._start_a_pulses < 0
419
                        or (self._start_a_pulses == 0 and self._dir_a < 0))
420
        self._start_angle = start_angle
421
        logging.debug("start angle {}, end angle {}, delta {}".format(
422
                      start_angle * 180.0 / math.pi,
423
                      end_angle * 180.0 / math.pi,
424
                      delta_angle * 180.0 / math.pi))
425
        delta_angle = abs(delta_angle)
426
        self._delta_angle = delta_angle
427

428
        # calculate values for interpolation.
429

430
        # calculate travel distance for axis in circular move.
431
        self._iterations_a = 0
432
        self._iterations_b = 0
433
        end_angle_m = end_angle
434
        if start_angle >= end_angle:
435
            end_angle_m += 2 * math.pi
436
        quarter_start = int(start_angle / (math.pi / 2.0))
437
        quarter_end = int(end_angle_m / (math.pi / 2.0))
438
        if quarter_end - quarter_start >= 4:
439
            self._iterations_a = 4 * round(radius * apm)
440
            self._iterations_b = 4 * round(radius * bpm)
441
        else:
442
            if quarter_start == quarter_end:
443
                self._iterations_a = round(abs(sa - ea) * apm)
444
                self._iterations_b = round(abs(sb - eb) * bpm)
445
            else:
446
                for r in range(quarter_start, quarter_end + 1):
447
                    i = r
448
                    if i >= 4:
449
                        i -= 4
450
                    if r == quarter_start:
451
                        if i == 0 or i == 2:
452
                            self._iterations_a += round(radius * apm) \
453
                                                  - round(abs(sa) * apm)
454
                        else:
455
                            self._iterations_a += round(abs(sa) * apm)
456
                        if i == 1 or i == 3:
457
                            self._iterations_b += round(radius * bpm) \
458
                                                  - round(abs(sb) * bpm)
459
                        else:
460
                            self._iterations_b += round(abs(sb) * bpm)
461
                    elif r == quarter_end:
462
                        if i == 0 or i == 2:
463
                            self._iterations_a += round(abs(ea) * apm)
464
                        else:
465
                            self._iterations_a += round(radius * apm) \
466
                                                  - round(abs(ea) * apm)
467
                        if i == 1 or i == 3:
468
                            self._iterations_b += round(abs(eb) * bpm)
469
                        else:
470
                            self._iterations_b += round(radius * bpm) \
471
                                                  - round(abs(eb) * bpm)
472
                    else:
473
                        self._iterations_a += round(radius * apm)
474
                        self._iterations_b += round(radius * bpm)
475
            if direction == CCW:
476
                self._iterations_a = (4 * round(radius * apm)
477
                                      - self._iterations_a)
478
                self._iterations_b = (4 * round(radius * bpm)
479
                                      - self._iterations_b)
480

481
        arc = delta_angle * radius
482
        e2 = delta.e * delta.e
483
        if self._plane == PLANE_XY:
484
            self._iterations_3rd = abs(delta.z) * STEPPER_PULSES_PER_MM_Z
485
            full_length = math.sqrt(arc * arc + delta.z * delta.z + e2)
486
            if full_length == 0:
487
                self._velocity_3rd = velocity
488
            else:
489
                self._velocity_3rd = abs(delta.z) / full_length * velocity
490
            self._third_dir = math.copysign(1, delta.z)
491
        elif self._plane == PLANE_YZ:
492
            self._iterations_3rd = abs(delta.x) * STEPPER_PULSES_PER_MM_X
493
            full_length = math.sqrt(arc * arc + delta.x * delta.x + e2)
494
            if full_length == 0:
495
                self._velocity_3rd = velocity
496
            else:
497
                self._velocity_3rd = abs(delta.x) / full_length * velocity
498
            self._third_dir = math.copysign(1, delta.x)
499
        elif self._plane == PLANE_ZX:
500
            self._iterations_3rd = abs(delta.y) * STEPPER_PULSES_PER_MM_Y
501
            full_length = math.sqrt(arc * arc + delta.y * delta.y + e2)
502
            if full_length == 0:
503
                self._velocity_3rd = velocity
504
            else:
505
                self._velocity_3rd = abs(delta.y) / full_length * velocity
506
            self._third_dir = math.copysign(1, delta.y)
507
        else:
508
            raise ValueError("Unknown plane")
509
        self._iterations_e = abs(delta.e) * STEPPER_PULSES_PER_MM_E
510
        # Velocity splits with corresponding distance.
511
        if full_length == 0:
512
            circular_velocity = velocity
513
            self._e_velocity = velocity
514
        else:
515
            circular_velocity = arc / full_length * velocity
516
            self._e_velocity = abs(delta.e) / full_length * velocity
517
        if self._plane == PLANE_XY:
518
            self.max_velocity_mm_per_sec = self._adjust_velocity(
519
                Coordinates(circular_velocity, circular_velocity,
520
                            self._velocity_3rd, self._e_velocity))
521
            circular_velocity = min(self.max_velocity_mm_per_sec.x,
522
                                    self.max_velocity_mm_per_sec.y)
523
            self._velocity_3rd = self.max_velocity_mm_per_sec.z
524
        elif self._plane == PLANE_YZ:
525
            self.max_velocity_mm_per_sec = self._adjust_velocity(
526
                Coordinates(self._velocity_3rd, circular_velocity,
527
                            circular_velocity, self._e_velocity))
528
            circular_velocity = min(self.max_velocity_mm_per_sec.y,
529
                                    self.max_velocity_mm_per_sec.z)
530
            self._velocity_3rd = self.max_velocity_mm_per_sec.x
531
        elif self._plane == PLANE_ZX:
532
            self.max_velocity_mm_per_sec = self._adjust_velocity(
533
                Coordinates(circular_velocity, self._velocity_3rd,
534
                            circular_velocity, self._e_velocity))
535
            circular_velocity = min(self.max_velocity_mm_per_sec.z,
536
                                    self.max_velocity_mm_per_sec.x)
537
            self._velocity_3rd = self.max_velocity_mm_per_sec.y
538
        self._e_velocity = self.max_velocity_mm_per_sec.e
539
        self._r_div_v = radius / circular_velocity
540
        self._e_dir = math.copysign(1, delta.e)
541
        self.acceleration_time_s = (self.max_velocity_mm_per_sec.find_max()
542
                                    / STEPPER_MAX_ACCELERATION_MM_PER_S2)
543
        if full_length == 0:
544
            self.linear_time_s = 0.0
545
            self.max_velocity_mm_per_sec = Coordinates(0, 0, 0, 0)
546
        elif STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s \
547
                ** 2 > full_length:
548
            self.acceleration_time_s = \
549
                math.sqrt(full_length / STEPPER_MAX_ACCELERATION_MM_PER_S2)
550
            self.linear_time_s = 0.0
551
            v = full_length / self.acceleration_time_s
552
            if self.max_velocity_mm_per_sec.x > 0.0:
553
                self.max_velocity_mm_per_sec.x = v
554
            if self.max_velocity_mm_per_sec.y > 0.0:
555
                self.max_velocity_mm_per_sec.y = v
556
            if self.max_velocity_mm_per_sec.z > 0.0:
557
                self.max_velocity_mm_per_sec.z = v
558
            if self.max_velocity_mm_per_sec.e > 0.0:
559
                self.max_velocity_mm_per_sec.e = v
560
        else:
561
            linear_distance_mm = full_length - self.acceleration_time_s ** 2 \
562
                                 * STEPPER_MAX_ACCELERATION_MM_PER_S2
563
            self.linear_time_s = linear_distance_mm / math.sqrt(
564
                circular_velocity ** 2 + self._velocity_3rd ** 2
565
                + self._e_velocity ** 2)
566

567
    @staticmethod
568
    def __angle(a, b):
569
        # Calculate angle of entry point (a, b) of circle with center in (0,0)
570
        angle = math.acos(b / math.sqrt(a * a + b * b))
571
        if a < 0:
572
            return 2 * math.pi - angle
573
        return angle
574

575
    def _get_movement_parameters(self):
576
        """ Return movement parameters, see super class for details.
577
        """
578
        return (self.acceleration_time_s,
579
                self.linear_time_s,
580
                self.max_velocity_mm_per_sec)
581

582
    @staticmethod
583
    def __circular_helper(start, i, radius, side, direction):
584
        np = start + direction * i
585
        if np > radius:
586
            np -= 2 * (np - radius)
587
            direction = -direction
588
            side = not side
589
        if np < -radius:
590
            np -= 2 * (np + radius)
591
            direction = -direction
592
            side = not side
593
        if np > radius:
594
            np -= 2 * (np - radius)
595
            direction = -direction
596
            side = not side
597
        return np, direction, side
598

599
    def __circular_find_time(self, a, b):
600
        angle = self.__angle(a, b)
601
        if self._direction == CW:
602
            delta_angle = angle - self._start_angle
603
        else:
604
            delta_angle = self._start_angle - angle
605
        if delta_angle <= 0:
606
            delta_angle += 2 * math.pi
607
        return self._r_div_v * delta_angle
608

609
    def __circular_a(self, i, pulses_per_mm):
610
        if i >= self._iterations_a:
611
            return self._dir_a, None
612
        a, direction, side = \
613
            self.__circular_helper(self._start_a_pulses, i + 1,
614
                                   self._radius_a_pulses,
615
                                   self._side_a, self._dir_a)
616
        a /= pulses_per_mm
617
        # first and last item can be slightly out of bound due float precision
618
        if i + 1 == self._iterations_a:
619
            return direction, self._r_div_v * self._delta_angle
620
        b = math.sqrt(self._radius_a2 - a * a)
621
        if side:
622
            b = -b
623
        return direction, self.__circular_find_time(a, b)
624

625
    def __circular_b(self, i, pulses_per_mm):
626
        if i >= self._iterations_b:
627
            return self._dir_b, None
628
        b, direction, side = \
629
            self.__circular_helper(self._start_b_pulses, i + 1,
630
                                   self._radius_b_pulses,
631
                                   self._side_b, self._dir_b)
632
        b /= pulses_per_mm
633
        # first and last item can be slightly out of bound due float precision
634
        if i + 1 == self._iterations_b:
635
            return direction, self._r_div_v * self._delta_angle
636
        a = math.sqrt(self._radius_b2 - b * b)
637
        if side:
638
            a = -a
639
        return direction, self.__circular_find_time(a, b)
640

641
    @staticmethod
642
    def __linear(i, total_i, pulses_per_mm, velocity):
643
        if i >= total_i:
644
            return None
645
        return i / pulses_per_mm / velocity
646

647
    def _interpolation_function(self, ix, iy, iz, ie):
648
        """ Calculate interpolation values for linear movement, see super class
649
            for details.
650
        """
651
        if self._plane == PLANE_XY:
652
            dx, tx = self.__circular_a(ix, STEPPER_PULSES_PER_MM_X)
653
            dy, ty = self.__circular_b(iy, STEPPER_PULSES_PER_MM_Y)
654
            tz = self.__linear(iz, self._iterations_3rd,
655
                               STEPPER_PULSES_PER_MM_Z, self._velocity_3rd)
656
            dz = self._third_dir
657
        elif self._plane == PLANE_YZ:
658
            dy, ty = self.__circular_a(iy, STEPPER_PULSES_PER_MM_Y)
659
            dz, tz = self.__circular_b(iz, STEPPER_PULSES_PER_MM_Z)
660
            tx = self.__linear(ix, self._iterations_3rd,
661
                               STEPPER_PULSES_PER_MM_X, self._velocity_3rd)
662
            dx = self._third_dir
663
        else:  # self._plane == PLANE_ZX:
664
            dz, tz = self.__circular_a(iz, STEPPER_PULSES_PER_MM_Z)
665
            dx, tx = self.__circular_b(ix, STEPPER_PULSES_PER_MM_X)
666
            ty = self.__linear(iy, self._iterations_3rd,
667
                               STEPPER_PULSES_PER_MM_Y, self._velocity_3rd)
668
            dy = self._third_dir
669
        te = self.__linear(ie, self._iterations_e, STEPPER_PULSES_PER_MM_E,
670
                           self._e_velocity)
671
        return (dx, dy, dz, self._e_dir), (tx, ty, tz, te)
672