From bb61c1243469f101ee0e55e3d15ebb72931b680a Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 08:53:12 -0500
Subject: [PATCH] use numpy array

---
 software/rpi/hexapod.py | 55 +++++++++++++++++++++++------------------
 software/rpi/lib.py     | 23 +++++++++--------
 2 files changed, 42 insertions(+), 36 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index 9bbbd9f..8c83c49 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -103,33 +103,33 @@ class Hexapod:
 
         for mm in range(0, 30):
             for idx in range(0, 20):
-                move_to = np.array([full_path[0][0][idx], full_path[0][1][idx], full_path[0][2][idx], full_path[0][3][idx], full_path[0][4][idx], full_path[0][5][idx]])+self.standby_coordinate
+                move_to = full_path[idx, :, :]+self.standby_coordinate
 
                 self.ik(move_to)
 
-                self.leg_0.set_angle(0, self.angles[0,0])
-                self.leg_0.set_angle(1, self.angles[0,1])
-                self.leg_0.set_angle(2, self.angles[0,2])
+                self.leg_0.set_angle(0, self.angles[0, 0])
+                self.leg_0.set_angle(1, self.angles[0, 1])
+                self.leg_0.set_angle(2, self.angles[0, 2])
 
-                self.leg_1.set_angle(0, self.angles[1,0])
-                self.leg_1.set_angle(1, self.angles[1,1])
-                self.leg_1.set_angle(2, self.angles[1,2])
+                self.leg_1.set_angle(0, self.angles[1, 0])
+                self.leg_1.set_angle(1, self.angles[1, 1])
+                self.leg_1.set_angle(2, self.angles[1, 2])
 
-                self.leg_2.set_angle(0, self.angles[2,0])
-                self.leg_2.set_angle(1, self.angles[2,1])
-                self.leg_2.set_angle(2, self.angles[2,2])
+                self.leg_2.set_angle(0, self.angles[2, 0])
+                self.leg_2.set_angle(1, self.angles[2, 1])
+                self.leg_2.set_angle(2, self.angles[2, 2])
 
-                self.leg_3.set_angle(0, self.angles[3,0])
-                self.leg_3.set_angle(1, self.angles[3,1])
-                self.leg_3.set_angle(2, self.angles[3,2])
+                self.leg_3.set_angle(0, self.angles[3, 0])
+                self.leg_3.set_angle(1, self.angles[3, 1])
+                self.leg_3.set_angle(2, self.angles[3, 2])
 
-                self.leg_4.set_angle(0, self.angles[4,0])
-                self.leg_4.set_angle(1, self.angles[4,1])
-                self.leg_4.set_angle(2, self.angles[4,2])
+                self.leg_4.set_angle(0, self.angles[4, 0])
+                self.leg_4.set_angle(1, self.angles[4, 1])
+                self.leg_4.set_angle(2, self.angles[4, 2])
 
-                self.leg_5.set_angle(0, self.angles[5,0])
-                self.leg_5.set_angle(1, self.angles[5,1])
-                self.leg_5.set_angle(2, self.angles[5,2])
+                self.leg_5.set_angle(0, self.angles[5, 0])
+                self.leg_5.set_angle(1, self.angles[5, 1])
+                self.leg_5.set_angle(2, self.angles[5, 2])
 
                 time.sleep(0.005)
 
@@ -138,7 +138,6 @@ class Hexapod:
         print(self.angles)
 
     def standby(self):
-        
 
         self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+(
             self.j2_j3*COS30)+self.j3_tip*SIN15)*np.cos(self.mount_angle)
@@ -204,12 +203,20 @@ class Hexapod:
         g_radius = 25
         halfsteps = int(g_steps/2)
 
-        path = semicircle_generator(g_radius, g_steps)
+        path = np.zeros((g_steps, 6, 3))
 
-        mir_path = deque(path)
-        mir_path.rotate(halfsteps)
+        path[:, 0, :] = semicircle_generator(g_radius, g_steps)
 
-        return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
+        mir_path = np.roll(path[:, 0, :], halfsteps, axis=0)
+        path[:, 2, :] = path[:, 0, :]
+        path[:, 4, :] = path[:, 0, :]
+        path[:, 1, :] = mir_path
+        path[:, 3, :] = mir_path
+        path[:, 5, :] = mir_path
+
+        return path
+
+        # return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
 
 
 def main():
diff --git a/software/rpi/lib.py b/software/rpi/lib.py
index f0d305c..47e08e6 100644
--- a/software/rpi/lib.py
+++ b/software/rpi/lib.py
@@ -9,25 +9,24 @@ def semicircle_generator(radius, steps, reverse=False):
 
     step_angle = np.pi / halfsteps
 
-    result = []
+    result = np.zeros((steps, 3))
+    halfsteps_array = np.arange(halfsteps)
 
     # first half, move backward (only y change)
-    for i in range(halfsteps):
-        result.append((0, radius - i*radius*2/(halfsteps), 0))
+    result[:halfsteps, 1] = radius - halfsteps_array*radius*2/(halfsteps)
 
     # second half, move forward in semicircle shape (y, z change)
-    for i in range(halfsteps):
-        angle = np.pi - step_angle*i
-        y = radius * math.cos(angle)
-        z = radius * math.sin(angle)
-        result.append((0, y, z))
+    angle = np.pi - step_angle*halfsteps_array
+    y = radius * np.cos(angle)
+    z = radius * np.sin(angle)
+    result[halfsteps:, 1] = y
+    result[halfsteps:, 2] = z
 
-    result = deque(result)
-    result.rotate(int(steps/4))
+    result = np.roll(result, int(steps/4), axis=0)
 
     if reverse:
-        result = deque(reversed(result))
-        result.rotate(1)
+        result = np.flip(result, axis=0)
+        result = np.roll(result, 1, axis=0)
 
     return result