From d4dc987b553d1caf85e715665ae5242b5688ed31 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 21:35:50 -0500
Subject: [PATCH] fast walk

---
 software/rpi/hexapod.py        | 13 +++++++--
 software/rpi/lib.py            | 26 ++++++++---------
 software/rpi/path_generator.py | 52 +++++++++++++++++++++++++++++++++-
 3 files changed, 75 insertions(+), 16 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index c6ed8db..df9e346 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -12,6 +12,7 @@ import numpy as np
 import time
 import json
 from path_generator import gen_forward_path, gen_backward_path
+from path_generator import gen_fastforward_path, gen_fastbackward_path
 
 
 SIN30 = 0.5
@@ -86,16 +87,24 @@ class Hexapod:
         self.standby_coordinate = self.calculate_standby_coordinate(60, 75)
         self.forward_path = gen_forward_path()
         self.backward_path = gen_backward_path()
+        self.fastforward_path = gen_fastforward_path()
+        self.fastbackward_path = gen_fastbackward_path()
 
         self.standby()
         time.sleep(1)
 
-        for mm in range(0, 30):
+        for mm in range(0, 20):
             self.move(self.forward_path, 0.005)
 
-        for mm in range(0, 30):
+        for mm in range(0, 20):
             self.move(self.backward_path, 0.005)
 
+        for mm in range(0, 20):
+            self.move(self.fastforward_path, 0.005)
+        
+        for mm in range(0, 20):
+            self.move(self.fastbackward_path, 0.005)
+
     def calculate_standby_coordinate(self, j2_angle, j3_angle):
         j2_rad = j2_angle/180*np.pi
         j3_rad = j3_angle/180*np.pi
diff --git a/software/rpi/lib.py b/software/rpi/lib.py
index 47e08e6..e132616 100644
--- a/software/rpi/lib.py
+++ b/software/rpi/lib.py
@@ -37,26 +37,26 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, 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, y_radius - i*y_radius*2/(halfsteps), 0))
+    result[:halfsteps, 1] = y_radius - halfsteps_array*y_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 = y_radius * math.cos(angle)
-        z = z_radius * math.sin(angle)
-        x = x_radius * math.sin(angle)
-        result.append((x, y, z))
+    angle = np.pi - step_angle*halfsteps_array
+    y = y_radius * np.cos(angle)
+    z = z_radius * np.sin(angle)
+    x = x_radius * np.sin(angle)
+    result[halfsteps:, 0] = x
+    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
 
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index 640c0d5..9f7d63e 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -1,4 +1,4 @@
-from lib import semicircle_generator
+from lib import semicircle_generator, semicircle2_generator
 import numpy as np
 
 
@@ -40,3 +40,53 @@ def gen_backward_path():
     path[:, 5, :] = mir_path
 
     return path
+
+
+def gen_fastforward_path():
+    g_steps = 20
+    y_radius = 50
+    z_radius = 30
+    x_radius = 10
+
+    halfsteps = int(g_steps/2)
+
+    path = np.zeros((g_steps, 6, 3))
+    path[:, 0, :] = semicircle2_generator(
+        g_steps, y_radius, z_radius, x_radius)
+    path[:, 4, :] = semicircle2_generator(
+        g_steps, y_radius, z_radius, -x_radius)
+
+    mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0)
+    path[:, 1, :] = mir_rpath
+    path[:, 2, :] = path[:, 0, :]
+
+    mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0)
+    path[:, 3, :] = mir_lpath
+    path[:, 5, :] = mir_lpath
+
+    return path
+
+
+def gen_fastbackward_path():
+    g_steps = 20
+    y_radius = 50
+    z_radius = 30
+    x_radius = 10
+
+    halfsteps = int(g_steps/2)
+
+    path = np.zeros((g_steps, 6, 3))
+    path[:, 0, :] = semicircle2_generator(
+        g_steps, y_radius, z_radius, x_radius, reverse=True)
+    path[:, 4, :] = semicircle2_generator(
+        g_steps, y_radius, z_radius, -x_radius, reverse=True)
+
+    mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0)
+    path[:, 1, :] = mir_rpath
+    path[:, 2, :] = path[:, 0, :]
+
+    mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0)
+    path[:, 3, :] = mir_lpath
+    path[:, 5, :] = mir_lpath
+
+    return path