Add CoreXY support

Author: Fabien-Chouteau

gcode/src/gcode-execution.adb

@@ -161,7 +161,7 @@ package body Gcode.Execution is
          return;
       end if;
 
-      Planner.Planner_Add_Dwell (Ctx, Duration (Ctx.B ('P').Value));
+      Planner.Planner_Add_Dwell (Ctx, Ctx.B ('P').Value);
    end Dwell_Command;
 
    -------------

gcode/src/gcode-planner.adb

@@ -27,7 +27,6 @@ with Settings; use Settings;
 
 package body Gcode.Planner is
 
-   use Float_Functions;
    use type Float_Position;
    use type Step_Position;
 
@@ -37,23 +36,22 @@ package body Gcode.Planner is
    type Motion_Block (Kind : Motion_Kind := Motion_Line) is record
       case Kind is
          when Motion_Line =>
-            Target           : Float_Position;
+            Target             : Float_Position;
 
-            Relative_Steps   : Step_Position;
-            --  Steps for each axis
+            Abs_Relative_Steps : Step_Position;
+            --  Absolute number of steps for each axis
 
-            Directions       : Axis_Directions := (others => Forward);
+            Directions         : Axis_Directions := (others => Forward);
             --  Step direction for each axis
 
-            Step_Event_Count : Steps := 0;
+            Step_Event_Count   : Steps := 0;
             --  Number of steps required to complete this block
 
-            Entry_Speed   : Step_Speed := 0.0;
-            Nominal_Speed : Step_Speed := 0.0;
-            Acceleration  : Step_Acceleration := Step_Acceleration'Last;
-            Remaining_Distance : Float_Value := 0.0;
+            Entry_Speed        : Step_Speed := 0.0;
+            Nominal_Speed      : Step_Speed := 0.0;
+            Acceleration       : Step_Acceleration := Step_Acceleration'Last;
          when Motion_Dwell =>
-            Dwell_Duration   : Duration;
+            Dwell_Duration     : Float_Value;
          when Motion_Homing =>
             null;
          when Motion_Enable_Motors =>
@@ -102,39 +100,48 @@ package body Gcode.Planner is
       pragma Unreferenced (Ctx);
       M_Block : Motion_Block (Kind => Motion_Line);
       Target_Steps : constant Step_Position := Milli_To_Step (Target);
+      Relative_Steps : Step_Position := Target_Steps - Planner_Position;
       Delta_MM : Float_Value;
 
       Unit_Vect : Float_Position;
       pragma Unreferenced (Unit_Vect);
       --  Unit vector of the block used for junction angle computation
    begin
 
-      M_Block.Relative_Steps := abs (Target_Steps - Planner_Position);
+      if Settings.Use_CoreXY_Motion then
+         declare
+            DX, DY, DA, DB : Steps;
+         begin
+            DX := Relative_Steps (X_Axis);
+            DY := Relative_Steps (Y_Axis);
+
+            DA := DX + DY;
+            DB := DX - DY;
+
+            Relative_Steps (X_Axis) := DA;
+            Relative_Steps (Y_Axis) := DB;
+         end;
+      end if;
+
+      M_Block.Abs_Relative_Steps := abs Relative_Steps;
 
       for Axis in Axis_Name loop
          M_Block.Step_Event_Count :=
            Steps'Max (M_Block.Step_Event_Count,
-                      M_Block.Relative_Steps (Axis));
+                      M_Block.Abs_Relative_Steps (Axis));
 
          --  Distance traveled in milimeters
          Delta_MM :=
-           Float_Value (Target_Steps (Axis) - Planner_Position (Axis))
+           Float_Value (Relative_Steps (Axis))
              / Settings.Step_Per_Millimeter (Axis);
 
          --  Direction of movement for this axis
          M_Block.Directions (Axis) :=
            (if Delta_MM < 0.0 then Backward else Forward);
 
-         --  Add square of each axis to compute distance
-         M_Block.Remaining_Distance :=
-           M_Block.Remaining_Distance + Delta_MM**2;
-
          Unit_Vect (Axis) := Delta_MM;
       end loop;
 
-      --  Compute distance
-      M_Block.Remaining_Distance := Sqrt (M_Block.Remaining_Distance);
-
       if M_Block.Step_Event_Count = 0 then
          --  zero-length block
          return;
@@ -156,7 +163,7 @@ package body Gcode.Planner is
 
    procedure Planner_Add_Dwell
      (Ctx            : in out GContext'Class;
-      Dwell_Duration : Duration)
+      Dwell_Duration : Float_Value)
    is
       pragma Unreferenced (Ctx);
       M_Block : Motion_Block (Kind => Motion_Dwell);
@@ -218,7 +225,7 @@ package body Gcode.Planner is
       Seg                  : Segment;
       Remaining_Steps      : Steps;
       Remaining_Seg        : Natural;
-      Seg_Required_To_Stop : Natural;
+      Seg_Required_To_Stop : Integer;
    begin
       loop
 
@@ -232,13 +239,13 @@ package body Gcode.Planner is
 
                --  100Hz gives us a 10ms precision
                Seg.Frequency :=
-                 Duration'Max (100.0, Settings.Idle_Stepper_Frequency);
+                 Frequency_Value'Max (100.0, Settings.Idle_Stepper_Frequency);
 
                Seg.Step_Count :=
                  Steps (Motion.Dwell_Duration * Seg.Frequency);
 
                --  No axis is moving
-               Seg.Block_Steps := (others => 0);
+               Seg.Abs_Block_Steps := (others => 0);
                Seg.Block_Event_Count := Steps'Last;
 
                --  Blocking call
@@ -254,14 +261,16 @@ package body Gcode.Planner is
             when Motion_Enable_Motors =>
                Segment_Block_Buffer.Insert ((Kind => Enable_Motors_Segment,
                                              Enable => Motion.Enable));
+
+
             when Motion_Line =>
                Remaining_Steps := Motion.Step_Event_Count;
 
                --  Signal this segment as first of the new block
                Seg.New_Block := True;
 
                --  Segment values that will remain for the entire block
-               Seg.Block_Steps := Motion.Relative_Steps;
+               Seg.Abs_Block_Steps := Motion.Abs_Relative_Steps;
                Seg.Block_Event_Count := Motion.Step_Event_Count;
                Seg.Directions := Motion.Directions;
 
@@ -307,7 +316,6 @@ package body Gcode.Planner is
 
                   --  Next segements are not first of the block
                   Seg.New_Block := False;
-
                end loop;
          end case;
       end loop;

gcode/src/gcode-planner.ads

@@ -31,7 +31,7 @@ package Gcode.Planner is
 
    procedure Planner_Add_Dwell
      (Ctx             : in out GContext'Class;
-      Dwell_Duration  : Duration);
+      Dwell_Duration  : Float_Value);
 
    procedure Planner_Add_Homing
      (Ctx       : in out GContext'Class;
@@ -59,15 +59,15 @@ package Gcode.Planner is
             Directions : Axis_Directions := (others => Forward);
             --  Step direction for each axis
 
-            Block_Steps : Step_Position;
-            --  Steps for the current Motion block each axis
+            Abs_Block_Steps : Step_Position;
+            --  Absolute number of steps for the current Motion block each axis
 
             Block_Event_Count : Steps;
             --  Step count for the current block
          when Homing_Segment =>
             null;
          when Dwell_Segment =>
-            Dwell_Duration : Duration;
+            Dwell_Duration : Float_Value;
          when Enable_Motors_Segment =>
             Enable : Motor_Enable_Array;
       end case;

gcode/src/gcode.adb

@@ -74,6 +74,18 @@ package body Gcode is
       return Ret;
    end Milli_To_Step;
 
+   -------------------
+   -- Milli_To_Step --
+   -------------------
+
+   function Milli_To_Step (S    : Float_Value;
+                           Axis : Axis_Name)
+                           return Steps
+   is
+   begin
+      return Steps (S * Settings.Step_Per_Millimeter (Axis));
+   end Milli_To_Step;
+
    -------------------
    -- Inch_To_Milli --
    -------------------

gcode/src/gcode.ads

@@ -27,7 +27,7 @@ package Gcode is
 
    subtype Float_Value is Float;
    subtype Line_Value is Natural;
-   subtype Frequency_Value is Duration;
+   subtype Frequency_Value is Float;
 
    package Float_Functions is new
      Ada.Numerics.Generic_Elementary_Functions (Float_Value);
@@ -59,6 +59,9 @@ package Gcode is
 
    function Step_To_Milli (S : Step_Position) return Float_Position;
    function Milli_To_Step (S : Float_Position) return Step_Position;
+   function Milli_To_Step (S    : Float_Value;
+                           Axis : Axis_Name)
+                           return Steps;
    function Inch_To_Milli (S : Float_Position) return Float_Position;
    function Inch_To_Milli (S : Float_Value) return Float_Value;