Add support for variable line widths.

9 files changed, 143 insertions, 18 deletions

diff --git a/ArcWelderInverseProcessor/ArcWelderInverseProcessor.cpp b/ArcWelderInverseProcessor/ArcWelderInverseProcessor.cpp
index ea333b3..ca64ff3 100644
--- a/ArcWelderInverseProcessor/ArcWelderInverseProcessor.cpp
+++ b/ArcWelderInverseProcessor/ArcWelderInverseProcessor.cpp
@@ -26,7 +26,7 @@
 #define _CRT_SECURE_NO_DEPRECATE
 #endif
 
-#include "inverse_processor.h"
+#include "marlin_2_arc.h"
 #include "ArcWelderInverseProcessor.h"
 #include <cstring>
 #include <iostream>
@@ -233,7 +233,7 @@ int main(int argc, char* argv[])
     target_file_path = temp_file_path;
   }
   
-  inverse_processor processor(source_file_path, target_file_path, g90_g91_influences_extruder, 50, cs);
+  marlin_2_arc processor(source_file_path, target_file_path, g90_g91_influences_extruder, 50, cs);
   processor.process();
   // Todo:  get some results!
   if (true)
diff --git a/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj b/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj
index 37ec7d0..f67571c 100644
--- a/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj
+++ b/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj
@@ -202,11 +202,15 @@
   </ItemDefinitionGroup>
   <ItemGroup>
     <ClInclude Include="ArcWelderInverseProcessor.h" />
-    <ClInclude Include="inverse_processor.h" />
+    <ClInclude Include="firmware_types.h" />
+    <ClInclude Include="marlin_2_arc.h" />
+    <ClInclude Include="repiter_arc.h" />
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="ArcWelderInverseProcessor.cpp" />
-    <ClCompile Include="inverse_processor.cpp" />
+    <ClCompile Include="firmware_types.cpp" />
+    <ClCompile Include="marlin_2_arc.cpp" />
+    <ClCompile Include="repiter_arc.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ProjectReference Include="..\ArcWelder\ArcWelder.vcxproj">
diff --git a/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj.filters b/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj.filters
index fe669d6..bc338f9 100644
--- a/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj.filters
+++ b/ArcWelderInverseProcessor/ArcWelderInverseProcessor.vcxproj.filters
@@ -18,7 +18,13 @@
     <ClInclude Include="ArcWelderInverseProcessor.h">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="inverse_processor.h">
+    <ClInclude Include="repiter_arc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="marlin_2_arc.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="firmware_types.h">
       <Filter>Header Files</Filter>
     </ClInclude>
   </ItemGroup>
@@ -26,7 +32,13 @@
     <ClCompile Include="ArcWelderInverseProcessor.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="inverse_processor.cpp">
+    <ClCompile Include="repiter_arc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="marlin_2_arc.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="firmware_types.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
   </ItemGroup>
diff --git a/ArcWelderInverseProcessor/firmware_types.cpp b/ArcWelderInverseProcessor/firmware_types.cpp
new file mode 100644
index 0000000..61820cf
--- /dev/null
+++ b/ArcWelderInverseProcessor/firmware_types.cpp
@@ -0,0 +1 @@
+#include "firmware_types.h"
diff --git a/ArcWelderInverseProcessor/firmware_types.h b/ArcWelderInverseProcessor/firmware_types.h
new file mode 100644
index 0000000..b3507ad
--- /dev/null
+++ b/ArcWelderInverseProcessor/firmware_types.h
@@ -0,0 +1,4 @@
+#pragma once
+enum firmware_types { Marlin2, Repiter };
+
+
diff --git a/ArcWelderInverseProcessor/inverse_processor.cpp b/ArcWelderInverseProcessor/marlin_2_arc.cpp
index c6edb6f..b7b3246 100644
--- a/ArcWelderInverseProcessor/inverse_processor.cpp
+++ b/ArcWelderInverseProcessor/marlin_2_arc.cpp
@@ -47,14 +47,14 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "inverse_processor.h"
+#include "marlin_2_arc.h"
 #include <cmath>
 
 //#include "Marlin.h"
 //#include "stepper.h"
 //#include "planner.h"
 
-inverse_processor::inverse_processor(std::string source_path, std::string target_path, bool g90_g91_influences_extruder, int buffer_size, ConfigurationStore cs)
+marlin_2_arc::marlin_2_arc(std::string source_path, std::string target_path, bool g90_g91_influences_extruder, int buffer_size, ConfigurationStore cs)
 {
     source_path_ = source_path;
     target_path_ = target_path;
@@ -79,7 +79,7 @@ inverse_processor::inverse_processor(std::string source_path, std::string target
 */
 }
 
-gcode_position_args inverse_processor::get_args_(bool g90_g91_influences_extruder, int buffer_size)
+gcode_position_args marlin_2_arc::get_args_(bool g90_g91_influences_extruder, int buffer_size)
 {
     gcode_position_args args;
     // Configure gcode_position_args
@@ -112,12 +112,12 @@ gcode_position_args inverse_processor::get_args_(bool g90_g91_influences_extrude
     return args;
 }
 
-inverse_processor::~inverse_processor()
+marlin_2_arc::~marlin_2_arc()
 {
     delete p_source_position_;
 }
 
-void inverse_processor::process()
+void marlin_2_arc::process()
 {
     // Create a stringstream we can use for messaging.
     std::stringstream stream;
@@ -239,7 +239,7 @@ void inverse_processor::process()
 
 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
 // segment is configured in settings.mm_per_arc_segment.  
-void inverse_processor::mc_arc(float* position, float* target, float* offset, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder)
+void marlin_2_arc::mc_arc(float* position, float* target, float* offset, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder)
 {
     // Extract the position to reduce indexing at the cost of a few bytes of mem
     float p_x = position[X_AXIS];
@@ -363,7 +363,7 @@ void inverse_processor::mc_arc(float* position, float* target, float* offset, fl
         int8_t count = 0;
         for (i = 1; i < segments; i++) { // Increment (segments-1)
 
-          if (count < cs_.n_arc_correction) {
+          if (count < cs_.n_arc_correction /*&& theta_per_segment > 0.001 */) {
             // Apply vector rotation matrix 
             r_axisi = r_axis_x * sin_T + r_axis_y * cos_T;
             r_axis_x = r_axis_x * cos_T - r_axis_y * sin_T;
@@ -404,13 +404,13 @@ void inverse_processor::mc_arc(float* position, float* target, float* offset, fl
     position[E_AXIS] = t_e;
 }
 
-void inverse_processor::clamp_to_software_endstops(float* target)
+void marlin_2_arc::clamp_to_software_endstops(float* target)
 {
     // Do nothing, just added to keep mc_arc identical to the firmware version
     return;
 }
 
-void inverse_processor::plan_buffer_line(float x, float y, bool has_z, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target)
+void marlin_2_arc::plan_buffer_line(float x, float y, bool has_z, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target)
 {
     std::stringstream stream;
     stream << std::fixed;
diff --git a/ArcWelderInverseProcessor/inverse_processor.h b/ArcWelderInverseProcessor/marlin_2_arc.h
index ab33133..a7a2ac3 100644
--- a/ArcWelderInverseProcessor/inverse_processor.h
+++ b/ArcWelderInverseProcessor/marlin_2_arc.h
@@ -62,10 +62,10 @@ struct ConfigurationStore {
 	int n_arc_correction;
 	
 };
-class inverse_processor {
+class marlin_2_arc {
 public:
-	inverse_processor(std::string source_path, std::string target_path, bool g90_g91_influences_extruder, int buffer_size, ConfigurationStore cs = ConfigurationStore());
-	virtual ~inverse_processor();
+	marlin_2_arc(std::string source_path, std::string target_path, bool g90_g91_influences_extruder, int buffer_size, ConfigurationStore cs = ConfigurationStore());
+	virtual ~marlin_2_arc();
 	void process();
 	void mc_arc(float* position, float* target, float* offset, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder);
 	
diff --git a/ArcWelderInverseProcessor/repiter_arc.cpp b/ArcWelderInverseProcessor/repiter_arc.cpp
new file mode 100644
index 0000000..83818a0
--- /dev/null
+++ b/ArcWelderInverseProcessor/repiter_arc.cpp
@@ -0,0 +1,97 @@
+#include "repiter_arc.h"
+
+
+/*
+// Arc function taken from grbl
+// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
+// segment is configured in settings.mm_per_arc_segment.
+void repiter_arc::arc(float* position, float* target, float* offset, float radius, uint8_t isclockwise) {
+  //   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
+  //   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
+  float center_axis0 = position[X_AXIS] + offset[X_AXIS];
+  float center_axis1 = position[Y_AXIS] + offset[Y_AXIS];
+  //float linear_travel = 0; //target[axis_linear] - position[axis_linear];
+  float extruder_travel = (Printer::destinationSteps[E_AXIS] - Printer::currentPositionSteps[E_AXIS]) * Printer::invAxisStepsPerMM[E_AXIS];
+  float r_axis0 = -offset[0]; // Radius vector from center to current location
+  float r_axis1 = -offset[1];
+  float rt_axis0 = target[0] - center_axis0;
+  float rt_axis1 = target[1] - center_axis1;
+  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+  float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
+  if ((!isclockwise && angular_travel <= 0.00001) || (isclockwise && angular_travel < -0.000001)) {
+    angular_travel += 2.0f * M_PI;
+  }
+  if (isclockwise) {
+    angular_travel -= 2.0f * M_PI;
+  }
+
+  float millimeters_of_travel = fabs(angular_travel) * radius; //hypot(angular_travel*radius, fabs(linear_travel));
+  if (millimeters_of_travel < 0.001f) {
+    return; // treat as succes because there is nothing to do;
+  }
+  //uint16_t segments = (radius>=BIG_ARC_RADIUS ? floor(millimeters_of_travel/MM_PER_ARC_SEGMENT_BIG) : floor(millimeters_of_travel/MM_PER_ARC_SEGMENT));
+  // Increase segment size if printing faster then computation speed allows
+  uint16_t segments = (Printer::feedrate > 60.0f ? floor(millimeters_of_travel / RMath::min(static_cast<float>(MM_PER_ARC_SEGMENT_BIG), Printer::feedrate * 0.01666f * static_cast<float>(MM_PER_ARC_SEGMENT))) : floor(millimeters_of_travel / static_cast<float>(MM_PER_ARC_SEGMENT)));
+  if (segments == 0)
+    segments = 1;
+
+  float theta_per_segment = angular_travel / segments;
+  //float linear_per_segment = linear_travel/segments;
+  float extruder_per_segment = extruder_travel / segments;
+
+  
+  // Vector rotation matrix values
+  float cos_T = 1 - 0.5 * theta_per_segment * theta_per_segment; // Small angle approximation
+  float sin_T = theta_per_segment;
+
+  float arc_target[4];
+  float sin_Ti;
+  float cos_Ti;
+  float r_axisi;
+  uint16_t i;
+  int8_t count = 0;
+
+  // Initialize the linear axis
+  //arc_target[axis_linear] = position[axis_linear];
+
+  // Initialize the extruder axis
+  arc_target[E_AXIS] = Printer::currentPositionSteps[E_AXIS] * Printer::invAxisStepsPerMM[E_AXIS];
+
+  for (i = 1; i < segments; i++) {
+    // Increment (segments-1)
+
+    if ((count & 3) == 0) {
+      //GCode::readFromSerial();
+      Commands::checkForPeriodicalActions(false);
+      UI_MEDIUM; // do check encoder
+    }
+
+    if (count < N_ARC_CORRECTION) { //25 pieces
+        // Apply vector rotation matrix
+      r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
+      r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
+      r_axis1 = r_axisi;
+      count++;
+    }
+    else {
+      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+      cos_Ti = cos(i * theta_per_segment);
+      sin_Ti = sin(i * theta_per_segment);
+      r_axis0 = -offset[0] * cos_Ti + offset[1] * sin_Ti;
+      r_axis1 = -offset[0] * sin_Ti - offset[1] * cos_Ti;
+      count = 0;
+    }
+
+    // Update arc_target location
+    arc_target[X_AXIS] = center_axis0 + r_axis0;
+    arc_target[Y_AXIS] = center_axis1 + r_axis1;
+    //arc_target[axis_linear] += linear_per_segment;
+    arc_target[E_AXIS] += extruder_per_segment;
+    Printer::moveToReal(arc_target[X_AXIS], arc_target[Y_AXIS], IGNORE_COORDINATE, arc_target[E_AXIS], IGNORE_COORDINATE);
+  }
+  // Ensure last segment arrives at target location.
+  Printer::moveToReal(target[X_AXIS], target[Y_AXIS], IGNORE_COORDINATE, target[E_AXIS], IGNORE_COORDINATE);
+}
+
+*/
\ No newline at end of file
diff --git a/ArcWelderInverseProcessor/repiter_arc.h b/ArcWelderInverseProcessor/repiter_arc.h
new file mode 100644
index 0000000..353c040
--- /dev/null
+++ b/ArcWelderInverseProcessor/repiter_arc.h
@@ -0,0 +1,7 @@
+#pragma once
+#include <cstdint>
+class repiter_arc
+{
+  void arc(float* position, float* target, float* offset, float radius, uint8_t isclockwise);
+};
+