Simplest motor position control algoriphm.

Author: godunko

source/control.adb

@@ -0,0 +1,53 @@
+--
+--  Copyright (C) 2025, Vadim Godunko <vgodunko@gmail.com>
+--
+--  SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
+--
+
+with Motor_Drivers;
+with Sensors;
+
+package body Control is
+
+   Desired : A0B.Types.Unsigned_16 := 2_048;
+
+   ----------------
+   -- Initialize --
+   ----------------
+
+   procedure Initialize is
+   begin
+      null;
+   end Initialize;
+
+   ---------------
+   -- Iteration --
+   ---------------
+
+   procedure Iteration is
+      use type A0B.Types.Unsigned_16;
+
+      Current : constant A0B.Types.Unsigned_16 := Sensors.Get_Position;
+
+   begin
+      if Current < Desired - 10 then
+         Motor_Drivers.Set_Forward (1);
+
+      elsif Current > Desired + 10 then
+         Motor_Drivers.Set_Backward (1);
+
+      else
+         Motor_Drivers.Set_Break (1);
+      end if;
+   end Iteration;
+
+   ---------
+   -- Set --
+   ---------
+
+   procedure Set (To : A0B.Types.Unsigned_16) is
+   begin
+      Desired := To;
+   end Set;
+
+end Control;

source/control.ads

@@ -0,0 +1,17 @@
+--
+--  Copyright (C) 2025, Vadim Godunko <vgodunko@gmail.com>
+--
+--  SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
+--
+
+with A0B.Types;
+
+package Control is
+
+   procedure Iteration;
+
+   procedure Initialize;
+
+   procedure Set (To : A0B.Types.Unsigned_16);
+
+end Control;

source/driver.adb

@@ -4,18 +4,25 @@
 --  SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
 --
 
+pragma Ada_2022;
+
+with A0B.Types;
+
 with Configuration;
 with Console;
-with Motor_Drivers;
+with Control;
 with Sensors;
 
 procedure Driver is
-   Motor   : Motor_Drivers.Motor_Index := 1;
+   use type A0B.Types.Unsigned_16;
+
    Command : Character;
+   Desired : A0B.Types.Unsigned_16 := 2_048;
 
 begin
    Configuration.Initialize;
    Sensors.Initialize;
+   Control.Initialize;
    Configuration.Enable_Timers;
 
    Console.New_Line;
@@ -27,43 +34,21 @@ begin
       Console.Get (Command);
 
       case Command is
-         when '1' =>
-            Console.Put_Line ("Motor 1 selected");
-            Motor := 1;
-
-         when '2' =>
-            Console.Put_Line ("Motor 2 selected");
-            Motor := 2;
-
-         when '3' =>
-            Console.Put_Line ("Motor 3 selected");
-            Motor := 3;
-
-         when '4' =>
-            Console.Put_Line ("Motor 4 selected");
-            Motor := 4;
-
-         when 'f' | 'F' =>
-            Console.Put_Line ("forward");
-            Motor_Drivers.Set_Forward (Motor);
-
-         when 'r' | 'R' =>
-            Console.Put_Line ("reverse");
-            Motor_Drivers.Set_Backward (Motor);
-
-         when 'b' | 'B' =>
-            Console.Put_Line ("break");
-            Motor_Drivers.Set_Break (Motor);
-
-         when 'o' | 'O' =>
-            Console.Put_Line ("off");
-            Motor_Drivers.Set_Off (Motor);
-
          when 'a' | 'A' =>
             Console.Put_Line ("collect sensors data");
             Sensors.Collect_Data;
             Sensors.Dump;
 
+         when '-' | '_' =>
+            Desired := @ - 16;
+            Control.Set (Desired);
+            Console.Put_Line (A0B.Types.Unsigned_16'Image (Desired));
+
+         when '=' | '+' =>
+            Desired := @ + 16;
+            Control.Set (Desired);
+            Console.Put_Line (A0B.Types.Unsigned_16'Image (Desired));
+
          when others =>
             Console.Put_Line ("unknown command");
       end case;

source/sensors.adb

@@ -7,10 +7,10 @@
 pragma Ada_2022;
 
 with A0B.Callbacks.Generic_Parameterless;
-with A0B.Types;
 
 with Configuration;
 with Console;
+with Control;
 
 package body Sensors is
 
@@ -35,6 +35,8 @@ package body Sensors is
    Last   : Natural := 0;
    --  Buffer to accumulate data.
 
+   Current : Sensors_Data;
+
    procedure On_Conversion_Done;
 
    package On_Conversion_Done_Callbacks is
@@ -107,6 +109,15 @@ package body Sensors is
       Console.New_Line;
    end Dump;
 
+   ------------------
+   -- Get_Position --
+   ------------------
+
+   function Get_Position return A0B.Types.Unsigned_16 is
+   begin
+      return Current.M1_Position;
+   end Get_Position;
+
    ----------------
    -- Initialize --
    ----------------
@@ -138,6 +149,8 @@ package body Sensors is
               Buffer (Buffer'First .. Buffer'Length / 2);
             Last := @ + Buffer'Length / 2;
          end if;
+
+         Current := Buffer (Buffer'Length / 2);
       end if;
 
       if Configuration.ADC1_DMA_Stream.Get_Masked_And_Clear_Transfer_Completed
@@ -147,7 +160,11 @@ package body Sensors is
               Buffer (Buffer'First + Buffer'Length / 2 .. Buffer'Last);
             Last := @ + Buffer'Length / 2;
          end if;
+
+         Current := Buffer (Buffer'Last);
       end if;
+
+      Control.Iteration;
    end On_Conversion_Done;
 
 end Sensors;

source/sensors.ads

@@ -4,6 +4,8 @@
 --  SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
 --
 
+with A0B.Types;
+
 package Sensors is
 
    procedure Collect_Data;
@@ -12,4 +14,6 @@ package Sensors is
 
    procedure Initialize;
 
+   function Get_Position return A0B.Types.Unsigned_16;
+
 end Sensors;