From d020a35a5da9a27b54a478fadf8c8f956081f62e Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Fri, 10 Dec 2021 16:40:20 -0500
Subject: [PATCH] update

---
 software/rpi/hexapod.py        |  4 ++--
 software/rpi/lib.py            | 17 ++++++-----------
 software/rpi/path_generator.py | 31 ++++++++++---------------------
 3 files changed, 18 insertions(+), 34 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index d7ddd0d..a3567f9 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -114,8 +114,8 @@ class Hexapod:
         for mm in range(0, 20):
             self.move(self.forward_path, 0.005)
 
-        # for mm in range(0, 20):
-        #     self.move(self.backward_path, 0.005)
+        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)
diff --git a/software/rpi/lib.py b/software/rpi/lib.py
index e132616..fd0401e 100644
--- a/software/rpi/lib.py
+++ b/software/rpi/lib.py
@@ -17,10 +17,8 @@ def semicircle_generator(radius, steps, reverse=False):
 
     # second half, move forward in semicircle shape (y, z change)
     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[halfsteps:, 1] = radius * np.cos(angle)
+    result[halfsteps:, 2] = radius * np.sin(angle)
 
     result = np.roll(result, int(steps/4), axis=0)
 
@@ -43,14 +41,11 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
     # first half, move backward (only y change)
     result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps)
 
-    # second half, move forward in semicircle shape (y, z change)
+    # second half, move forward in semicircle shape (x, y, z change)
     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[halfsteps:, 0] = x_radius * np.sin(angle)
+    result[halfsteps:, 1] = y_radius * np.cos(angle)
+    result[halfsteps:, 2] = z_radius * np.sin(angle)
 
     result = np.roll(result, int(steps/4), axis=0)
 
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index b72e43d..76aa0de 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -5,22 +5,15 @@ import numpy as np
 from collections import deque
 
 
-def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25):
+def gen_forward_path(standby_coordinate, g_steps=20, g_radius=25):
+    assert (g_steps % 4) == 0
+
     halfsteps = int(g_steps/2)
-    g_steps = halfsteps*2
 
     path = np.zeros((g_steps, 6, 3))
 
-    # path[:, 0, :] = semicircle_generator(g_radius, g_steps)
     semi_circle = semicircle_generator(g_radius, g_steps)
 
-    # 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
-
     path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
     path[:, [1, 3, 5], :] = np.tile(
         np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
@@ -28,22 +21,18 @@ def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25):
     return path+np.tile(standby_coordinate, (g_steps, 1, 1))
 
 
-def gen_backward_path(standby_coordinate):
-    # assert (g_steps % 4) == 0
-    g_steps = 20
-    g_radius = 25
+def gen_backward_path(standby_coordinate, g_steps=20, g_radius=25):
+    assert (g_steps % 4) == 0
+
     halfsteps = int(g_steps/2)
 
     path = np.zeros((g_steps, 6, 3))
 
-    path[:, 0, :] = semicircle_generator(g_radius, g_steps, reverse=True)
+    semi_circle = semicircle_generator(g_radius, g_steps, reverse=True)
 
-    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
+    path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
+    path[:, [1, 3, 5], :] = np.tile(
+        np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
 
     return path+np.tile(standby_coordinate, (g_steps, 1, 1))