Просмотр исходного кода

Implement COREXZ in stepper.cpp and planner.cpp

Scott Lahteine 10 лет назад
Родитель
Сommit
9f53e2f0c9
2 измененных файлов: 131 добавлений и 85 удалений
  1. 25
    0
      Marlin/planner.cpp
  2. 106
    85
      Marlin/stepper.cpp

+ 25
- 0
Marlin/planner.cpp Просмотреть файл

@@ -542,6 +542,11 @@ float junction_deviation = 0.1;
542 542
     block->steps[A_AXIS] = labs(dx + dy);
543 543
     block->steps[B_AXIS] = labs(dx - dy);
544 544
     block->steps[Z_AXIS] = labs(dz);
545
+  #elif defined(COREXZ)
546
+    // corexz planning
547
+    block->steps[A_AXIS] = labs(dx + dz);
548
+    block->steps[Y_AXIS] = labs(dy);
549
+    block->steps[C_AXIS] = labs(dx - dz);
545 550
   #else
546 551
     // default non-h-bot planning
547 552
     block->steps[X_AXIS] = labs(dx);
@@ -572,6 +577,12 @@ float junction_deviation = 0.1;
572 577
     if (dz < 0) db |= BIT(Z_AXIS);
573 578
     if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
574 579
     if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
580
+  #elif defined(COREXZ)
581
+    if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
582
+    if (dy < 0) db |= BIT(Y_AXIS);
583
+    if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
584
+    if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
585
+    if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
575 586
   #else
576 587
     if (dx < 0) db |= BIT(X_AXIS);
577 588
     if (dy < 0) db |= BIT(Y_AXIS); 
@@ -591,6 +602,11 @@ float junction_deviation = 0.1;
591 602
     #ifndef Z_LATE_ENABLE
592 603
       if (block->steps[Z_AXIS]) enable_z();
593 604
     #endif
605
+  #elif defined(COREXZ)
606
+    if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
607
+      enable_x();
608
+      enable_z();
609
+    }
594 610
   #else
595 611
     if (block->steps[X_AXIS]) enable_x();
596 612
     if (block->steps[Y_AXIS]) enable_y();
@@ -683,6 +699,13 @@ float junction_deviation = 0.1;
683 699
     delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
684 700
     delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
685 701
     delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
702
+  #elif defined(COREXZ)
703
+    float delta_mm[6];
704
+    delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
705
+    delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
706
+    delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
707
+    delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
708
+    delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
686 709
   #else
687 710
     float delta_mm[4];
688 711
     delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
@@ -698,6 +721,8 @@ float junction_deviation = 0.1;
698 721
     block->millimeters = sqrt(
699 722
       #ifdef COREXY
700 723
         square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
724
+      #elif defined(COREXZ)
725
+        square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
701 726
       #else
702 727
         square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
703 728
       #endif

+ 106
- 85
Marlin/stepper.cpp Просмотреть файл

@@ -342,34 +342,38 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
342 342
   return timer;
343 343
 }
344 344
 
345
-// set the stepper direction of each axis
345
+/**
346
+ * Set the stepper direction of each axis
347
+ *
348
+ *   X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY
349
+ *   X_AXIS=A_AXIS and Z_AXIS=C_AXIS for COREXZ
350
+ */
346 351
 void set_stepper_direction() {
347
-  
348
-  // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
349
-  if (TEST(out_bits, X_AXIS)) {
350
-    X_APPLY_DIR(INVERT_X_DIR,0);
352
+
353
+  if (TEST(out_bits, X_AXIS)) { // A_AXIS
354
+    X_APPLY_DIR(INVERT_X_DIR, 0);
351 355
     count_direction[X_AXIS] = -1;
352 356
   }
353 357
   else {
354
-    X_APPLY_DIR(!INVERT_X_DIR,0);
358
+    X_APPLY_DIR(!INVERT_X_DIR, 0);
355 359
     count_direction[X_AXIS] = 1;
356 360
   }
357 361
 
358
-  if (TEST(out_bits, Y_AXIS)) {
359
-    Y_APPLY_DIR(INVERT_Y_DIR,0);
362
+  if (TEST(out_bits, Y_AXIS)) { // B_AXIS
363
+    Y_APPLY_DIR(INVERT_Y_DIR, 0);
360 364
     count_direction[Y_AXIS] = -1;
361 365
   }
362 366
   else {
363
-    Y_APPLY_DIR(!INVERT_Y_DIR,0);
367
+    Y_APPLY_DIR(!INVERT_Y_DIR, 0);
364 368
     count_direction[Y_AXIS] = 1;
365 369
   }
366 370
   
367
-  if (TEST(out_bits, Z_AXIS)) {
368
-    Z_APPLY_DIR(INVERT_Z_DIR,0);
371
+  if (TEST(out_bits, Z_AXIS)) { // C_AXIS
372
+    Z_APPLY_DIR(INVERT_Z_DIR, 0);
369 373
     count_direction[Z_AXIS] = -1;
370 374
   }
371 375
   else {
372
-    Z_APPLY_DIR(!INVERT_Z_DIR,0);
376
+    Z_APPLY_DIR(!INVERT_Z_DIR, 0);
373 377
     count_direction[Z_AXIS] = 1;
374 378
   }
375 379
   
@@ -503,6 +507,11 @@ ISR(TIMER1_COMPA_vect) {
503 507
         // If DeltaX == -DeltaY, the movement is only in Y axis
504 508
         if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS))) {
505 509
           if (TEST(out_bits, X_HEAD))
510
+      #elif defined(COREXZ)
511
+        // Head direction in -X axis for CoreXZ bots.
512
+        // If DeltaX == -DeltaZ, the movement is only in Z axis
513
+        if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, C_AXIS))) {
514
+          if (TEST(out_bits, X_HEAD))
506 515
       #else
507 516
           if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular Cartesian bot)
508 517
       #endif
@@ -528,8 +537,11 @@ ISR(TIMER1_COMPA_vect) {
528 537
                 #endif
529 538
               }
530 539
           }
531
-      #ifdef COREXY
540
+      #if defined(COREXY) || defined(COREXZ)
532 541
         }
542
+      #endif
543
+
544
+      #ifdef COREXY
533 545
         // Head direction in -Y axis for CoreXY bots.
534 546
         // If DeltaX == DeltaY, the movement is only in X axis
535 547
         if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS))) {
@@ -547,82 +559,91 @@ ISR(TIMER1_COMPA_vect) {
547 559
               UPDATE_ENDSTOP(Y, MAX);
548 560
             #endif
549 561
           }
550
-      #ifdef COREXY
562
+      #if defined(COREXY) || defined(COREXZ)
551 563
         }
552 564
       #endif
553
-      if (TEST(out_bits, Z_AXIS)) { // z -direction
554
-        #if HAS_Z_MIN
555
-
556
-          #ifdef Z_DUAL_ENDSTOPS
557
-            SET_ENDSTOP_BIT(Z, MIN);
558
-              #if HAS_Z2_MIN
559
-                SET_ENDSTOP_BIT(Z2, MIN);
560
-              #else
561
-                COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
562
-              #endif
563
-
564
-            byte z_test = TEST_ENDSTOP(Z_MIN) << 0 + TEST_ENDSTOP(Z2_MIN) << 1; // bit 0 for Z, bit 1 for Z2
565
-
566
-            if (z_test && current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN
567
-              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
568
-              endstop_hit_bits |= BIT(Z_MIN);
569
-              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
570
-                step_events_completed = current_block->step_event_count;
571
-            }
572
-          #else // !Z_DUAL_ENDSTOPS
573
-
574
-            UPDATE_ENDSTOP(Z, MIN);
575
-          #endif // !Z_DUAL_ENDSTOPS
576
-        #endif // Z_MIN_PIN
577
-
578
-        #ifdef Z_PROBE_ENDSTOP
579
-          UPDATE_ENDSTOP(Z, PROBE);
580
-
581
-          if (TEST_ENDSTOP(Z_PROBE))
582
-          {
583
-            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
584
-            endstop_hit_bits |= BIT(Z_PROBE);
565
+
566
+      #ifdef COREXZ
567
+        // Head direction in -Z axis for CoreXZ bots.
568
+        // If DeltaX == DeltaZ, the movement is only in X axis
569
+        if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, C_AXIS))) {
570
+          if (TEST(out_bits, Z_HEAD))
571
+      #else
572
+          if (TEST(out_bits, Z_AXIS))
573
+      #endif
574
+          { // z -direction
575
+            #if HAS_Z_MIN
576
+
577
+              #ifdef Z_DUAL_ENDSTOPS
578
+                SET_ENDSTOP_BIT(Z, MIN);
579
+                  #if HAS_Z2_MIN
580
+                    SET_ENDSTOP_BIT(Z2, MIN);
581
+                  #else
582
+                    COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
583
+                  #endif
584
+
585
+                byte z_test = TEST_ENDSTOP(Z_MIN) << 0 + TEST_ENDSTOP(Z2_MIN) << 1; // bit 0 for Z, bit 1 for Z2
586
+
587
+                if (z_test && current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN
588
+                  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
589
+                  endstop_hit_bits |= BIT(Z_MIN);
590
+                  if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
591
+                    step_events_completed = current_block->step_event_count;
592
+                }
593
+              #else // !Z_DUAL_ENDSTOPS
594
+
595
+                UPDATE_ENDSTOP(Z, MIN);
596
+              #endif // !Z_DUAL_ENDSTOPS
597
+            #endif // Z_MIN_PIN
598
+
599
+            #ifdef Z_PROBE_ENDSTOP
600
+              UPDATE_ENDSTOP(Z, PROBE);
601
+
602
+              if (TEST_ENDSTOP(Z_PROBE))
603
+              {
604
+                endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
605
+                endstop_hit_bits |= BIT(Z_PROBE);
606
+              }
607
+            #endif
585 608
           }
586
-        #endif
587
-      }
588
-      else { // z +direction
589
-        #if HAS_Z_MAX
590
-
591
-          #ifdef Z_DUAL_ENDSTOPS
592
-
593
-            SET_ENDSTOP_BIT(Z, MAX);
594
-              #if HAS_Z2_MAX
595
-                SET_ENDSTOP_BIT(Z2, MAX);
596
-              #else
597
-                COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX)
598
-              #endif
599
-
600
-            byte z_test = TEST_ENDSTOP(Z_MAX) << 0 + TEST_ENDSTOP(Z2_MAX) << 1; // bit 0 for Z, bit 1 for Z2
601
-
602
-            if (z_test && current_block->steps[Z_AXIS] > 0) {  // t_test = Z_MAX || Z2_MAX
603
-              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
604
-              endstop_hit_bits |= BIT(Z_MIN);
605
-              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
606
-                step_events_completed = current_block->step_event_count;
607
-            }
608
-
609
-          #else // !Z_DUAL_ENDSTOPS
610
-
611
-            UPDATE_ENDSTOP(Z, MAX);
612
-
613
-          #endif // !Z_DUAL_ENDSTOPS
614
-        #endif // Z_MAX_PIN
615
-        
616
-        #ifdef Z_PROBE_ENDSTOP
617
-          UPDATE_ENDSTOP(Z, PROBE);
618
-          
619
-          if (TEST_ENDSTOP(Z_PROBE))
620
-          {
621
-            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
622
-            endstop_hit_bits |= BIT(Z_PROBE);
609
+          else { // z +direction
610
+            #if HAS_Z_MAX
611
+
612
+              #ifdef Z_DUAL_ENDSTOPS
613
+
614
+                SET_ENDSTOP_BIT(Z, MAX);
615
+                  #if HAS_Z2_MAX
616
+                    SET_ENDSTOP_BIT(Z2, MAX);
617
+                  #else
618
+                    COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX)
619
+                  #endif
620
+
621
+                byte z_test = TEST_ENDSTOP(Z_MAX) << 0 + TEST_ENDSTOP(Z2_MAX) << 1; // bit 0 for Z, bit 1 for Z2
622
+
623
+                if (z_test && current_block->steps[Z_AXIS] > 0) {  // t_test = Z_MAX || Z2_MAX
624
+                  endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
625
+                  endstop_hit_bits |= BIT(Z_MIN);
626
+                  if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
627
+                    step_events_completed = current_block->step_event_count;
628
+                }
629
+
630
+              #else // !Z_DUAL_ENDSTOPS
631
+
632
+                UPDATE_ENDSTOP(Z, MAX);
633
+
634
+              #endif // !Z_DUAL_ENDSTOPS
635
+            #endif // Z_MAX_PIN
636
+            
637
+            #ifdef Z_PROBE_ENDSTOP
638
+              UPDATE_ENDSTOP(Z, PROBE);
639
+              
640
+              if (TEST_ENDSTOP(Z_PROBE))
641
+              {
642
+                endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
643
+                endstop_hit_bits |= BIT(Z_PROBE);
644
+              }
645
+            #endif
623 646
           }
624
-        #endif
625
-      }
626 647
       old_endstop_bits = current_endstop_bits;
627 648
     }
628 649
 

Загрузка…
Отмена
Сохранить