Sfoglia il codice sorgente

Implement BEZIER_JERK_CONTROL

Enable 6th-order jerk-controlled motion planning in real-time.
Only for 32bit MCUs. (AVR simply does not have enough processing power for this!)
etagle 6 anni fa
parent
commit
a29adde5c0

+ 1
- 0
.travis.yml Vedi File

@@ -436,6 +436,7 @@ script:
436 436
   - export TEST_PLATFORM="-e DUE"
437 437
   - restore_configs
438 438
   - opt_set MOTHERBOARD BOARD_RAMPS4DUE_EFB
439
+  - opt_set BEZIER_JERK_CONTROL
439 440
   - cp Marlin/Configuration.h Marlin/src/config/default/Configuration.h
440 441
   - cp Marlin/Configuration_adv.h Marlin/src/config/default/Configuration_adv.h
441 442
   - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}

+ 11
- 0
Marlin/Configuration.h Vedi File

@@ -608,6 +608,17 @@
608 608
 #define DEFAULT_ZJERK                  0.3
609 609
 #define DEFAULT_EJERK                  5.0
610 610
 
611
+/**
612
+ * Realtime Jerk Control
613
+ *
614
+ * This option eliminates vibration during printing by fitting a Bézier
615
+ * curve to move acceleration, producing much smoother direction changes.
616
+ * Because this is computationally-intensive, a 32-bit MCU is required.
617
+ *
618
+ * See https://github.com/synthetos/TinyG/wiki/Jerk-Controlled-Motion-Explained
619
+ */
620
+//#define BEZIER_JERK_CONTROL
621
+
611 622
 //===========================================================================
612 623
 //============================= Z Probe Options =============================
613 624
 //===========================================================================

+ 1
- 1
Marlin/src/backtrace/unwarm_thumb.cpp Vedi File

@@ -260,7 +260,7 @@ UnwResult UnwStartThumb(UnwState * const state) {
260 260
 
261 261
         UnwPrintd5("TB%c [r%d,r%d%s]\n", H ? 'H' : 'B', rn, rm, H ? ",LSL #1" : "");
262 262
 
263
-        // We are only interested if the RN is the PC. Let´s choose the 1st destination
263
+        // We are only interested if the RN is the PC. Let's choose the 1st destination
264 264
         if (rn == 15) {
265 265
           if (H) {
266 266
             uint16_t rv;

+ 1
- 1
Marlin/src/backtrace/unwinder.cpp Vedi File

@@ -28,7 +28,7 @@ extern "C" const UnwTabEntry __exidx_end[];
28 28
 
29 29
 // Detect if unwind information is present or not
30 30
 static int HasUnwindTableInfo(void) {
31
-  // > 16 because there are default entries we can´t supress
31
+  // > 16 because there are default entries we can't supress
32 32
   return ((char*)(&__exidx_end) - (char*)(&__exidx_start)) > 16 ? 1 : 0;
33 33
 }
34 34
 

+ 2
- 0
Marlin/src/inc/SanityCheck.h Vedi File

@@ -99,6 +99,8 @@
99 99
   #error "Z_ENDSTOP_SERVO_NR is now Z_PROBE_SERVO_NR. Please update your configuration."
100 100
 #elif defined(DEFAULT_XYJERK)
101 101
   #error "DEFAULT_XYJERK is deprecated. Use DEFAULT_XJERK and DEFAULT_YJERK instead."
102
+#elif ENABLED(BEZIER_JERK_CONTROL) && !defined(CPU_32_BIT)
103
+  #error "BEZIER_JERK_CONTROL is computationally intensive and requires a 32-bit board."
102 104
 #elif defined(XY_TRAVEL_SPEED)
103 105
   #error "XY_TRAVEL_SPEED is deprecated. Use XY_PROBE_SPEED instead."
104 106
 #elif defined(PROBE_SERVO_DEACTIVATION_DELAY)

+ 27
- 1
Marlin/src/module/planner.cpp Vedi File

@@ -229,6 +229,10 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
229 229
   NOLESS(initial_rate, MINIMAL_STEP_RATE);
230 230
   NOLESS(final_rate, MINIMAL_STEP_RATE);
231 231
 
232
+  #if ENABLED(BEZIER_JERK_CONTROL)
233
+    uint32_t cruise_rate = initial_rate;
234
+  #endif
235
+
232 236
   const int32_t accel = block->acceleration_steps_per_s2;
233 237
 
234 238
           // Steps required for acceleration, deceleration to/from nominal rate
@@ -246,16 +250,36 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
246 250
     NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
247 251
     accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
248 252
     plateau_steps = 0;
253
+
254
+    #if ENABLED(BEZIER_JERK_CONTROL)
255
+      // We won't reach the cruising rate. Let's calculate the speed we will reach
256
+      cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
257
+    #endif
249 258
   }
259
+  #if ENABLED(BEZIER_JERK_CONTROL)
260
+    else // We have some plateau time, so the cruise rate will be the nominal rate
261
+      cruise_rate = block->nominal_rate;
262
+  #endif
250 263
 
251 264
   // block->accelerate_until = accelerate_steps;
252 265
   // block->decelerate_after = accelerate_steps+plateau_steps;
253 266
 
267
+  #if ENABLED(BEZIER_JERK_CONTROL)
268
+    // Jerk controlled speed requires to express speed versus time, NOT steps
269
+    int32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * HAL_STEPPER_TIMER_RATE,
270
+            deceleration_time = ((float)(cruise_rate - final_rate) / accel) * HAL_STEPPER_TIMER_RATE;
271
+  #endif
272
+
254 273
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
255 274
   if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
256 275
     block->accelerate_until = accelerate_steps;
257 276
     block->decelerate_after = accelerate_steps + plateau_steps;
258 277
     block->initial_rate = initial_rate;
278
+    #if ENABLED(BEZIER_JERK_CONTROL)
279
+      block->acceleration_time = acceleration_time;
280
+      block->deceleration_time = deceleration_time;
281
+      block->cruise_rate = cruise_rate;
282
+    #endif
259 283
     block->final_rate = final_rate;
260 284
   }
261 285
   CRITICAL_SECTION_END;
@@ -1303,7 +1327,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
1303 1327
   }
1304 1328
   block->acceleration_steps_per_s2 = accel;
1305 1329
   block->acceleration = accel / steps_per_mm;
1306
-  block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
1330
+  #if DISABLED(BEZIER_JERK_CONTROL)
1331
+    block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
1332
+  #endif
1307 1333
   #if ENABLED(LIN_ADVANCE)
1308 1334
     if (block->use_advance_lead) {
1309 1335
       block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);

+ 19
- 3
Marlin/src/module/planner.h Vedi File

@@ -90,9 +90,17 @@ typedef struct {
90 90
     uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
91 91
   #endif
92 92
 
93
+  // Settings for the trapezoid generator
93 94
   int32_t accelerate_until,                 // The index of the step event on which to stop acceleration
94
-          decelerate_after,                 // The index of the step event on which to start decelerating
95
-          acceleration_rate;                // The acceleration rate used for acceleration calculation
95
+          decelerate_after;                 // The index of the step event on which to start decelerating
96
+
97
+  #if ENABLED(BEZIER_JERK_CONTROL)
98
+    uint32_t cruise_rate;                   // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
99
+    int32_t acceleration_time,              // Acceleration time and deceleration time in STEP timer counts
100
+            deceleration_time;
101
+  #else
102
+    int32_t acceleration_rate;              // The acceleration rate used for acceleration calculation
103
+  #endif
96 104
 
97 105
   uint8_t direction_bits;                   // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
98 106
 
@@ -112,7 +120,6 @@ typedef struct {
112 120
         millimeters,                        // The total travel of this block in mm
113 121
         acceleration;                       // acceleration mm/sec^2
114 122
 
115
-  // Settings for the trapezoid generator
116 123
   uint32_t nominal_rate,                    // The nominal step rate for this block in step_events/sec
117 124
            initial_rate,                    // The jerk-adjusted step rate at start of block
118 125
            final_rate,                      // The minimal rate at exit
@@ -639,6 +646,15 @@ class Planner {
639 646
       return SQRT(sq(target_velocity) - 2 * accel * distance);
640 647
     }
641 648
 
649
+    #if ENABLED(BEZIER_JERK_CONTROL)
650
+      /**
651
+       * Calculate the speed reached given initial speed, acceleration and distance
652
+       */
653
+      static float final_speed(const float &initial_velocity, const float &accel, const float &distance) {
654
+        return SQRT(sq(initial_velocity) + 2 * accel * distance);
655
+      }
656
+    #endif
657
+
642 658
     static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
643 659
 
644 660
     static void reverse_pass_kernel(block_t* const current, const block_t * const next);

+ 1
- 1
Marlin/src/module/planner_bezier.h Vedi File

@@ -23,7 +23,7 @@
23 23
 /**
24 24
  * planner_bezier.h
25 25
  *
26
- * Compute and buffer movement commands for bezier curves
26
+ * Compute and buffer movement commands for zier curves
27 27
  *
28 28
  */
29 29
 

+ 318
- 32
Marlin/src/module/stepper.cpp Vedi File

@@ -44,6 +44,13 @@
44 44
 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
45 45
    and Philipp Tiefenbacher. */
46 46
 
47
+/* Jerk controlled movements planner added by Eduardo José Tagle in April
48
+   2018, Equations based on Synthethos TinyG2 sources, but the fixed-point
49
+   implementation is a complete new one, as we are running the ISR with a
50
+   variable period.
51
+   Also implemented the Bézier velocity curve evaluation in ARM assembler,
52
+   to avoid impacting ISR speed. */
53
+
47 54
 #include "stepper.h"
48 55
 
49 56
 #ifdef __AVR__
@@ -109,6 +116,15 @@ long Stepper::counter_X = 0,
109 116
 
110 117
 volatile uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
111 118
 
119
+#if ENABLED(BEZIER_JERK_CONTROL)
120
+  int32_t Stepper::bezier_A,        // A coefficient in Bézier speed curve
121
+          Stepper::bezier_B,        // B coefficient in Bézier speed curve
122
+          Stepper::bezier_C,        // C coefficient in Bézier speed curve
123
+          Stepper::bezier_F;        // F coefficient in Bézier speed curve
124
+  uint32_t Stepper::bezier_AV;      // AV coefficient in Bézier speed curve
125
+  bool Stepper::bezier_2nd_half;    // =false If Bézier curve has been initialized or not
126
+#endif
127
+
112 128
 #if ENABLED(LIN_ADVANCE)
113 129
 
114 130
   uint32_t Stepper::LA_decelerate_after;
@@ -134,9 +150,9 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
134 150
 
135 151
 #endif // LIN_ADVANCE
136 152
 
137
-long Stepper::acceleration_time, Stepper::deceleration_time;
153
+int32_t Stepper::acceleration_time, Stepper::deceleration_time;
138 154
 
139
-volatile long Stepper::count_position[NUM_AXIS] = { 0 };
155
+volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
140 156
 volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
141 157
 
142 158
 #if ENABLED(MIXING_EXTRUDER)
@@ -145,8 +161,10 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
145 161
 
146 162
 uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
147 163
 
148
-hal_timer_t Stepper::OCR1A_nominal,
149
-            Stepper::acc_step_rate; // needed for deceleration start point
164
+hal_timer_t Stepper::OCR1A_nominal;
165
+#if DISABLED(BEZIER_JERK_CONTROL)
166
+  hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point
167
+#endif
150 168
 
151 169
 volatile long Stepper::endstops_trigsteps[XYZ];
152 170
 
@@ -298,6 +316,207 @@ void Stepper::set_directions() {
298 316
   extern volatile uint8_t e_hit;
299 317
 #endif
300 318
 
319
+#if ENABLED(BEZIER_JERK_CONTROL)
320
+  /**
321
+   *   We are using a quintic (fifth-degree) Bézier polynomial for the velocity curve.
322
+   *  This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position:
323
+   *  velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
324
+   *
325
+   *  The Bézier curve takes the form:
326
+   *
327
+   *  V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t)
328
+   *
329
+   *   Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are the control points, and B_0(t)
330
+   *  through B_5(t) are the Bernstein basis as follows:
331
+   *
332
+   *        B_0(t) =   (1-t)^5        =   -t^5 +  5t^4 - 10t^3 + 10t^2 -  5t   +   1
333
+   *        B_1(t) =  5(1-t)^4 * t    =   5t^5 - 20t^4 + 30t^3 - 20t^2 +  5t
334
+   *        B_2(t) = 10(1-t)^3 * t^2  = -10t^5 + 30t^4 - 30t^3 + 10t^2
335
+   *        B_3(t) = 10(1-t)^2 * t^3  =  10t^5 - 20t^4 + 10t^3
336
+   *        B_4(t) =  5(1-t)   * t^4  =  -5t^5 +  5t^4
337
+   *        B_5(t) =             t^5  =    t^5
338
+   *                                      ^       ^       ^       ^       ^       ^
339
+   *                                      |       |       |       |       |       |
340
+   *                                      A       B       C       D       E       F
341
+   *
342
+   *   Unfortunately, we cannot use forward-differencing to calculate each position through
343
+   *  the curve, as Marlin uses variable timer periods. So, we require a formula of the form:
344
+   *
345
+   *        V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F
346
+   *
347
+   *   Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5
348
+   *  through t of the Bézier form of V(t), we can determine that:
349
+   *
350
+   *        A =    -P_0 +  5*P_1 - 10*P_2 + 10*P_3 -  5*P_4 +  P_5
351
+   *        B =   5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 +  5*P_4
352
+   *        C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3
353
+   *        D =  10*P_0 - 20*P_1 + 10*P_2
354
+   *        E = - 5*P_0 +  5*P_1
355
+   *        F =     P_0
356
+   *
357
+   *   Now, since we will (currently) *always* want the initial acceleration and jerk values to be 0,
358
+   *  We set P_i = P_0 = P_1 = P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target velocity),
359
+   *  which, after simplification, resolves to:
360
+   *
361
+   *        A = - 6*P_i +  6*P_t =  6*(P_t - P_i)
362
+   *        B =  15*P_i - 15*P_t = 15*(P_i - P_t)
363
+   *        C = -10*P_i + 10*P_t = 10*(P_t - P_i)
364
+   *        D = 0
365
+   *        E = 0
366
+   *        F = P_i
367
+   *
368
+   *   As the t is evaluated in non uniform steps here, there is no other way rather than evaluating
369
+   *  the Bézier curve at each point:
370
+   *
371
+   *        V_f(t) = A*t^5 + B*t^4 + C*t^3 + F          [0 <= t <= 1]
372
+   *
373
+   *   Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
374
+   * use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
375
+   * per second (driver pulses should at least be 2uS hi/2uS lo), and allocating 2 bits to avoid
376
+   * overflows on the evaluation of the Bézier curve, means we can use
377
+   *
378
+   *   t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
379
+   *   A:   signed Q24.7 ,            |range = +/- 250000 * 6 * 128 = +/- 192000000 = 0x0B71B000 | 28 bits + sign
380
+   *   B:   signed Q24.7 ,            |range = +/- 250000 *15 * 128 = +/- 480000000 = 0x1C9C3800 | 29 bits + sign
381
+   *   C:   signed Q24.7 ,            |range = +/- 250000 *10 * 128 = +/- 320000000 = 0x1312D000 | 29 bits + sign
382
+   *   F:   signed Q24.7 ,            |range = +/- 250000     * 128 =      32000000 = 0x01E84800 | 25 bits + sign
383
+   *
384
+   *  The trapezoid generator state contains the following information, that we will use to create and evaluate
385
+   * the Bézier curve:
386
+   *
387
+   *  blk->step_event_count [TS] = The total count of steps for this movement. (=distance)
388
+   *  blk->initial_rate     [VI] = The initial steps per second (=velocity)
389
+   *  blk->final_rate       [VF] = The ending steps per second  (=velocity)
390
+   *  and the count of events completed (step_events_completed) [CS] (=distance until now)
391
+   *
392
+   *  Note the abbreviations we use in the following formulae are between []s
393
+   *
394
+   *  At the start of each trapezoid, we calculate the coefficients A,B,C,F and Advance [AV], as follows:
395
+   *
396
+   *   A =  6*128*(VF - VI) =  768*(VF - VI)
397
+   *   B = 15*128*(VI - VF) = 1920*(VI - VF)
398
+   *   C = 10*128*(VF - VI) = 1280*(VF - VI)
399
+   *   F =    128*VI        =  128*VI
400
+   *  AV = (1<<32)/TS      ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits)
401
+   *
402
+   *  And for each point, we will evaluate the curve with the following sequence:
403
+   *
404
+   *    uint32_t t = bezier_AV * curr_step;               // t: Range 0 - 1^32 = 32 bits
405
+   *    uint64_t f = t;
406
+   *    f *= t;                                           // Range 32*2 = 64 bits (unsigned)
407
+   *    f >>= 32;                                         // Range 32 bits  (unsigned)
408
+   *    f *= t;                                           // Range 32*2 = 64 bits  (unsigned)
409
+   *    f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
410
+   *    int64_t acc = (int64_t) bezier_F << 31;           // Range 63 bits (signed)
411
+   *    acc += ((uint32_t) f >> 1) * (int64_t) bezier_C;  // Range 29bits + 31 = 60bits (plus sign)
412
+   *    f *= t;                                           // Range 32*2 = 64 bits
413
+   *    f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
414
+   *    acc += ((uint32_t) f >> 1) * (int64_t) bezier_B;  // Range 29bits + 31 = 60bits (plus sign)
415
+   *    f *= t;                                           // Range 32*2 = 64 bits
416
+   *    f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
417
+   *    acc += ((uint32_t) f >> 1) * (int64_t) bezier_A;  // Range 28bits + 31 = 59bits (plus sign)
418
+   *    acc >>= (31 + 7);                                 // Range 24bits (plus sign)
419
+   *
420
+   * This can be translated to the following ARM assembly sequence:
421
+   *
422
+   * At start:
423
+   *  fhi = AV, flo = CS, alo = F
424
+   *
425
+   *  muls  fhi,flo               | f = AV * CS       1 cycles
426
+   *  mov   t,fhi                 | t = AV * CS       1 cycles
427
+   *  lsrs  ahi,alo,#1            | a  = F << 31      1 cycles
428
+   *  lsls  alo,alo,#31           |                   1 cycles
429
+   *  umull flo,fhi,fhi,t         | f *= t            5 cycles [fhi:flo=64bits
430
+   *  umull flo,fhi,fhi,t         | f>>=32; f*=t      5 cycles [fhi:flo=64bits
431
+   *  lsrs  flo,fhi,#1            |                   1 cycles [31bits
432
+   *  smlal alo,ahi,flo,C         | a+=(f>>33)*C;     5 cycles
433
+   *  umull flo,fhi,fhi,t         | f>>=32; f*=t      5 cycles [fhi:flo=64bits
434
+   *  lsrs  flo,fhi,#1            |                   1 cycles [31bits
435
+   *  smlal alo,ahi,flo,B         | a+=(f>>33)*B;     5 cycles
436
+   *  umull flo,fhi,fhi,t         | f>>=32; f*=t      5 cycles [fhi:flo=64bits
437
+   *  lsrs  flo,fhi,#1            | f>>=33;           1 cycles [31bits
438
+   *  smlal alo,ahi,flo,A         | a+=(f>>33)*A;     5 cycles
439
+   *  lsrs  alo,ahi,#6            | a>>=38            1 cycles
440
+   *  43 cycles total
441
+   */
442
+
443
+  FORCE_INLINE void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t interval) {
444
+    // Calculate the Bézier coefficients
445
+    bezier_A =  768 * (v1 - v0);
446
+    bezier_B = 1920 * (v0 - v1);
447
+    bezier_C = 1280 * (v1 - v0);
448
+    bezier_F =  128 * v0;
449
+    bezier_AV = 0xFFFFFFFF / interval;
450
+  }
451
+
452
+  FORCE_INLINE int32_t Stepper::_eval_bezier_curve(const uint32_t curr_step) {
453
+    #if defined(__ARM__) || defined(__thumb__)
454
+
455
+      // For ARM CORTEX M3/M4 CPUs, we have the optimized assembler version, that takes 43 cycles to execute
456
+      register uint32_t flo = 0;
457
+      register uint32_t fhi = bezier_AV * curr_step;
458
+      register uint32_t t = fhi;
459
+      register int32_t alo = bezier_F;
460
+      register int32_t ahi = 0;
461
+      register int32_t A = bezier_A;
462
+      register int32_t B = bezier_B;
463
+      register int32_t C = bezier_C;
464
+
465
+       __asm__ __volatile__(
466
+        ".syntax unified"                   "\n\t"  // is to prevent CM0,CM1 non-unified syntax
467
+        " lsrs  %[ahi],%[alo],#1"           "\n\t"  // a  = F << 31      1 cycles
468
+        " lsls  %[alo],%[alo],#31"          "\n\t"  //                   1 cycles
469
+        " umull %[flo],%[fhi],%[fhi],%[t]"  "\n\t"  // f *= t            5 cycles [fhi:flo=64bits]
470
+        " umull %[flo],%[fhi],%[fhi],%[t]"  "\n\t"  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
471
+        " lsrs  %[flo],%[fhi],#1"           "\n\t"  //                   1 cycles [31bits]
472
+        " smlal %[alo],%[ahi],%[flo],%[C]"  "\n\t"  // a+=(f>>33)*C;     5 cycles
473
+        " umull %[flo],%[fhi],%[fhi],%[t]"  "\n\t"  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
474
+        " lsrs  %[flo],%[fhi],#1"           "\n\t"  //                   1 cycles [31bits]
475
+        " smlal %[alo],%[ahi],%[flo],%[B]"  "\n\t"  // a+=(f>>33)*B;     5 cycles
476
+        " umull %[flo],%[fhi],%[fhi],%[t]"  "\n\t"  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
477
+        " lsrs  %[flo],%[fhi],#1"           "\n\t"  // f>>=33;           1 cycles [31bits]
478
+        " smlal %[alo],%[ahi],%[flo],%[A]"  "\n\t"  // a+=(f>>33)*A;     5 cycles
479
+        " lsrs  %[alo],%[ahi],#6"           "\n\t"  // a>>=38            1 cycles
480
+        : [alo]"+r"( alo ) ,
481
+          [flo]"+r"( flo ) ,
482
+          [fhi]"+r"( fhi ) ,
483
+          [ahi]"+r"( ahi ) ,
484
+          [A]"+r"( A ) ,  // <== Note: Even if A, B, C, and t registers are INPUT ONLY
485
+          [B]"+r"( B ) ,  //  GCC does bad optimizations on the code if we list them as
486
+          [C]"+r"( C ) ,  //  such, breaking this function. So, to avoid that problem,
487
+          [t]"+r"( t )    //  we list all registers as input-outputs.
488
+        :
489
+        : "cc"
490
+      );
491
+      return alo;
492
+
493
+    #else
494
+
495
+      // For non ARM targets, we provide a fallback implementation. Really doubt it
496
+      // will be useful, unless the processor is extremely fast.
497
+
498
+      uint32_t t = bezier_AV * curr_step;               // t: Range 0 - 1^32 = 32 bits
499
+      uint64_t f = t;
500
+      f *= t;                                           // Range 32*2 = 64 bits (unsigned)
501
+      f >>= 32;                                         // Range 32 bits  (unsigned)
502
+      f *= t;                                           // Range 32*2 = 64 bits  (unsigned)
503
+      f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
504
+      int64_t acc = (int64_t) bezier_F << 31;           // Range 63 bits (signed)
505
+      acc += ((uint32_t) f >> 1) * (int64_t) bezier_C;  // Range 29bits + 31 = 60bits (plus sign)
506
+      f *= t;                                           // Range 32*2 = 64 bits
507
+      f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
508
+      acc += ((uint32_t) f >> 1) * (int64_t) bezier_B;  // Range 29bits + 31 = 60bits (plus sign)
509
+      f *= t;                                           // Range 32*2 = 64 bits
510
+      f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
511
+      acc += ((uint32_t) f >> 1) * (int64_t) bezier_A;  // Range 28bits + 31 = 59bits (plus sign)
512
+      acc >>= (31 + 7);                                 // Range 24bits (plus sign)
513
+      return (int32_t) acc;
514
+
515
+    #endif
516
+  }
517
+
518
+#endif // BEZIER_JERK_CONTROL
519
+
301 520
 /**
302 521
  * Stepper Driver Interrupt
303 522
  *
@@ -394,26 +613,73 @@ void Stepper::isr() {
394 613
 
395 614
   // If there is no current block, attempt to pop one from the buffer
396 615
   if (!current_block) {
616
+
397 617
     // Anything in the buffer?
398 618
     if ((current_block = planner.get_current_block())) {
399
-      trapezoid_generator_reset();
619
+
620
+      // Initialize the trapezoid generator from the current block.
621
+      static int8_t last_extruder = -1;
622
+
623
+      #if ENABLED(LIN_ADVANCE)
624
+        #if E_STEPPERS > 1
625
+          if (current_block->active_extruder != last_extruder) {
626
+            current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
627
+            LA_active_extruder = current_block->active_extruder;
628
+          }
629
+        #endif
630
+
631
+        if ((use_advance_lead = current_block->use_advance_lead)) {
632
+          LA_decelerate_after = current_block->decelerate_after;
633
+          final_adv_steps = current_block->final_adv_steps;
634
+          max_adv_steps = current_block->max_adv_steps;
635
+        }
636
+      #endif
637
+
638
+      if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
639
+        last_direction_bits = current_block->direction_bits;
640
+        last_extruder = current_block->active_extruder;
641
+        set_directions();
642
+      }
643
+
644
+      // No acceleration / deceleration time elapsed so far
645
+      acceleration_time = deceleration_time = 0;
646
+
647
+      // No step events completed so far
648
+      step_events_completed = 0;
649
+
650
+      // step_rate to timer interval
651
+      OCR1A_nominal = calc_timer_interval(current_block->nominal_rate);
652
+
653
+      // make a note of the number of step loops required at nominal speed
654
+      step_loops_nominal = step_loops;
655
+
656
+      #if DISABLED(BEZIER_JERK_CONTROL)
657
+        // Set as deceleration point the initial rate of the block
658
+        acc_step_rate = current_block->initial_rate;
659
+      #endif
660
+
661
+      #if ENABLED(BEZIER_JERK_CONTROL)
662
+        // Initialize the Bézier speed curve
663
+        _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time);
664
+
665
+        // We have not started the 2nd half of the trapezoid
666
+        bezier_2nd_half = false;
667
+      #endif
400 668
 
401 669
       // Initialize Bresenham counters to 1/2 the ceiling
402 670
       counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
403
-
404 671
       #if ENABLED(MIXING_EXTRUDER)
405 672
         MIXING_STEPPERS_LOOP(i)
406 673
           counter_m[i] = -(current_block->mix_event_count[i] >> 1);
407 674
       #endif
408 675
 
409
-      step_events_completed = 0;
410
-
411 676
       #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
412 677
         e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
413 678
                    // No 'change' can be detected.
414 679
       #endif
415 680
 
416 681
       #if ENABLED(Z_LATE_ENABLE)
682
+        // If delayed Z enable, postpone move for 1mS
417 683
         if (current_block->steps[Z_AXIS] > 0) {
418 684
           enable_Z();
419 685
           _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
@@ -423,6 +689,7 @@ void Stepper::isr() {
423 689
       #endif
424 690
     }
425 691
     else {
692
+      // If no more queued moves, postpone next check for 1mS
426 693
       _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
427 694
       HAL_ENABLE_ISRs();
428 695
       return;
@@ -542,7 +809,6 @@ void Stepper::isr() {
542 809
     #endif
543 810
 
544 811
     #if ENABLED(LIN_ADVANCE)
545
-
546 812
       counter_E += current_block->steps[E_AXIS];
547 813
       if (counter_E > 0) {
548 814
         #if DISABLED(MIXING_EXTRUDER)
@@ -640,15 +906,23 @@ void Stepper::isr() {
640 906
   // Calculate new timer value
641 907
   if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
642 908
 
643
-    #ifdef CPU_32_BIT
644
-      MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
909
+    #if ENABLED(BEZIER_JERK_CONTROL)
910
+      // Get the next speed to use (Jerk limited!)
911
+      hal_timer_t acc_step_rate =
912
+        acceleration_time < current_block->acceleration_time
913
+          ? _eval_bezier_curve(acceleration_time)
914
+          : current_block->cruise_rate;
645 915
     #else
646
-      MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
647
-    #endif
648
-    acc_step_rate += current_block->initial_rate;
916
+      #ifdef CPU_32_BIT
917
+        MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
918
+      #else
919
+        MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
920
+      #endif
921
+      acc_step_rate += current_block->initial_rate;
649 922
 
650
-    // upper limit
651
-    NOMORE(acc_step_rate, current_block->nominal_rate);
923
+      // upper limit
924
+      NOMORE(acc_step_rate, current_block->nominal_rate);
925
+    #endif
652 926
 
653 927
     // step_rate to timer interval
654 928
     const hal_timer_t interval = calc_timer_interval(acc_step_rate);
@@ -659,7 +933,6 @@ void Stepper::isr() {
659 933
     acceleration_time += interval;
660 934
 
661 935
     #if ENABLED(LIN_ADVANCE)
662
-
663 936
       if (current_block->use_advance_lead) {
664 937
         if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
665 938
           nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
@@ -670,23 +943,40 @@ void Stepper::isr() {
670 943
         eISR_Rate = ADV_NEVER;
671 944
         if (e_steps) nextAdvanceISR = 0;
672 945
       }
673
-
674 946
     #endif // LIN_ADVANCE
675 947
   }
676 948
   else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
677 949
     hal_timer_t step_rate;
678
-    #ifdef CPU_32_BIT
679
-      MultiU32X24toH32(step_rate, deceleration_time, current_block->acceleration_rate);
950
+
951
+    #if ENABLED(BEZIER_JERK_CONTROL)
952
+      // If this is the 1st time we process the 2nd half of the trapezoid...
953
+      if (!bezier_2nd_half) {
954
+
955
+        // Initialize the Bézier speed curve
956
+        _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time);
957
+        bezier_2nd_half = true;
958
+      }
959
+
960
+      // Calculate the next speed to use
961
+      step_rate = deceleration_time < current_block->deceleration_time
962
+        ? _eval_bezier_curve(deceleration_time)
963
+        : current_block->final_rate;
680 964
     #else
681
-      MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
682
-    #endif
683 965
 
684
-    if (step_rate < acc_step_rate) { // Still decelerating?
685
-      step_rate = acc_step_rate - step_rate;
686
-      NOLESS(step_rate, current_block->final_rate);
687
-    }
688
-    else
689
-      step_rate = current_block->final_rate;
966
+      // Using the old trapezoidal control
967
+      #ifdef CPU_32_BIT
968
+        MultiU32X24toH32(step_rate, deceleration_time, current_block->acceleration_rate);
969
+      #else
970
+        MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
971
+      #endif
972
+
973
+      if (step_rate < acc_step_rate) { // Still decelerating?
974
+        step_rate = acc_step_rate - step_rate;
975
+        NOLESS(step_rate, current_block->final_rate);
976
+      }
977
+      else
978
+        step_rate = current_block->final_rate;
979
+    #endif
690 980
 
691 981
     // step_rate to timer interval
692 982
     const hal_timer_t interval = calc_timer_interval(step_rate);
@@ -697,7 +987,6 @@ void Stepper::isr() {
697 987
     deceleration_time += interval;
698 988
 
699 989
     #if ENABLED(LIN_ADVANCE)
700
-
701 990
       if (current_block->use_advance_lead) {
702 991
         if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
703 992
           nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
@@ -708,16 +997,13 @@ void Stepper::isr() {
708 997
         eISR_Rate = ADV_NEVER;
709 998
         if (e_steps) nextAdvanceISR = 0;
710 999
       }
711
-
712 1000
     #endif // LIN_ADVANCE
713 1001
   }
714 1002
   else {
715 1003
 
716 1004
     #if ENABLED(LIN_ADVANCE)
717
-
718 1005
       // If we have esteps to execute, fire the next advance_isr "now"
719 1006
       if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
720
-
721 1007
     #endif
722 1008
 
723 1009
     SPLIT(OCR1A_nominal);  // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL

+ 19
- 41
Marlin/src/module/stepper.h Vedi File

@@ -97,6 +97,15 @@ class Stepper {
97 97
     static long counter_X, counter_Y, counter_Z, counter_E;
98 98
     static volatile uint32_t step_events_completed; // The number of step events executed in the current block
99 99
 
100
+    #if ENABLED(BEZIER_JERK_CONTROL)
101
+      static int32_t bezier_A,        // A coefficient in Bézier speed curve
102
+                     bezier_B,        // B coefficient in Bézier speed curve
103
+                     bezier_C,        // C coefficient in Bézier speed curve
104
+                     bezier_F;        // F coefficient in Bézier speed curve
105
+      static uint32_t bezier_AV;      // AV coefficient in Bézier speed curve
106
+      static bool bezier_2nd_half;    // If Bézier curve has been initialized or not
107
+    #endif
108
+
100 109
     #if ENABLED(LIN_ADVANCE)
101 110
 
102 111
       static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early".
@@ -117,11 +126,13 @@ class Stepper {
117 126
 
118 127
     #endif // !LIN_ADVANCE
119 128
 
120
-    static long acceleration_time, deceleration_time;
129
+    static int32_t acceleration_time, deceleration_time;
121 130
     static uint8_t step_loops, step_loops_nominal;
122 131
 
123
-    static hal_timer_t OCR1A_nominal,
124
-                       acc_step_rate; // needed for deceleration start point
132
+    static hal_timer_t OCR1A_nominal;
133
+    #if DISABLED(BEZIER_JERK_CONTROL)
134
+      static hal_timer_t acc_step_rate; // needed for deceleration start point
135
+    #endif
125 136
 
126 137
     static volatile long endstops_trigsteps[XYZ];
127 138
     static volatile long endstops_stepsTotal, endstops_stepsDone;
@@ -129,7 +140,7 @@ class Stepper {
129 140
     //
130 141
     // Positions of stepper motors, in step units
131 142
     //
132
-    static volatile long count_position[NUM_AXIS];
143
+    static volatile int32_t count_position[NUM_AXIS];
133 144
 
134 145
     //
135 146
     // Current direction of stepper motors (+1 or -1)
@@ -349,43 +360,10 @@ class Stepper {
349 360
       return timer;
350 361
     }
351 362
 
352
-    // Initialize the trapezoid generator from the current block.
353
-    // Called whenever a new block begins.
354
-    FORCE_INLINE static void trapezoid_generator_reset() {
355
-
356
-      static int8_t last_extruder = -1;
357
-
358
-      #if ENABLED(LIN_ADVANCE)
359
-        #if E_STEPPERS > 1
360
-          if (current_block->active_extruder != last_extruder) {
361
-            current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
362
-            LA_active_extruder = current_block->active_extruder;
363
-          }
364
-        #endif
365
-
366
-        if ((use_advance_lead = current_block->use_advance_lead)) {
367
-          LA_decelerate_after = current_block->decelerate_after;
368
-          final_adv_steps = current_block->final_adv_steps;
369
-          max_adv_steps = current_block->max_adv_steps;
370
-        }
371
-      #endif
372
-
373
-      if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
374
-        last_direction_bits = current_block->direction_bits;
375
-        last_extruder = current_block->active_extruder;
376
-        set_directions();
377
-      }
378
-
379
-      deceleration_time = 0;
380
-      // step_rate to timer interval
381
-      OCR1A_nominal = calc_timer_interval(current_block->nominal_rate);
382
-      // make a note of the number of step loops required at nominal speed
383
-      step_loops_nominal = step_loops;
384
-      acc_step_rate = current_block->initial_rate;
385
-      acceleration_time = calc_timer_interval(acc_step_rate);
386
-      _NEXT_ISR(acceleration_time);
387
-
388
-    }
363
+    #if ENABLED(BEZIER_JERK_CONTROL)
364
+      static void _calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t steps);
365
+      static int32_t _eval_bezier_curve(const uint32_t curr_step);
366
+    #endif
389 367
 
390 368
     #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
391 369
       static void digipot_init();

Loading…
Annulla
Salva