From ed7915386d39177a4b43733157facc156894d71e Mon Sep 17 00:00:00 2001
From: Pierre Tellier <piair338@gmail.com>
Date: Mon, 31 Mar 2025 19:50:14 +0200
Subject: [PATCH] feat: added x and y movement

---
 control.py | 33 +++++++++++++++++----------------
 1 file changed, 17 insertions(+), 16 deletions(-)

diff --git a/control.py b/control.py
index 91c7938..75f9130 100644
--- a/control.py
+++ b/control.py
@@ -205,38 +205,39 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
     - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
     """
     # t = t*speed_x * 20
-    num_patte = 4
+    num_patte = 6
     slider_max = 0.200
 
     neutral_position = np.array([
-        [0.1, 0.15, -0.16],
-        [-0.1, 0.15, -0.16],
-        [-0.2, -0.00, -0.16],
-        [-0.1, -0.15, -0.16],
-        [0.1, -0.15, -0.16],
-        [0.2, 0, -0.16]
+        [0.1, 0.15, -0.15],
+        [-0.1, 0.15, -0.15],
+        [-0.2, -0.00, -0.15],
+        [-0.1, -0.15, -0.15],
+        [0.1, -0.15, -0.15],
+        [0.2, 0, -0.15]
     ])
 
     real_position = np.copy(neutral_position)
 
     movement_x = np.array([
-        [0.0, 0, 0],
         [0.00, 0, 0],
-        [-0.00, 0, 0],
+        [0.04, 0, 0],
+        [-0.04, 0, 0],
     ])
 
     movement_y = np.array([
         [0.0, 0, 0],
-        [0, 0.00, 0],
-        [0, -0.00, 0],
+        [0, 0.04, 0],
+        [0, -0.04, 0],
     ])
 
     movement_z = np.array([
-        [0, 0, 0],
-        [0, 0, 0],
-        [0, 0, 0]
+        [0, 0, 0.08],
+        [0, 0, -0.02],
+        [0, 0, -0.02]
     ])
 
+    # duration of each step of the movement
     step_duration = np.array([0.05, 0.3, 0.05])
 
     step_count = len(movement_z)
@@ -263,13 +264,13 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
         return floor((get_current_step(t) + 1) % step_count)
 
     def rotate(patte):
-        return [1, -1, -1, 1][
+        return [1, -1, 0, -1, 1, 0][
             patte] * movement_x  # + [-1, 1, -1, 1][patte] * movement_y # mettre des 0 partout sur le Y fait une très belle rotation
 
     def normalize(matrix, slider_max, speed):
         return (matrix / slider_max) * speed
 
-    offsets = np.array([0, 0.5, 0, 0.5]) * movement_duration  # offset between each leg
+    offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
     assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
 
     for patte in range(num_patte):