From ebeb098713cca6089f17c8996c6d088e36b5f2b2 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 22:32:45 -0500
Subject: [PATCH] shift left and right

---
 software/rpi/hexapod.py        | 14 +++++++++--
 software/rpi/path_generator.py | 44 ++++++++++++++++++++++++++++++++++
 2 files changed, 56 insertions(+), 2 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index bec076b..26bbe23 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -14,6 +14,7 @@ import json
 from path_generator import gen_forward_path, gen_backward_path
 from path_generator import gen_fastforward_path, gen_fastbackward_path
 from path_generator import gen_leftturn_path, gen_rightturn_path
+from path_generator import gen_shiftleft_path, gen_shiftright_path
 
 
 SIN30 = 0.5
@@ -94,6 +95,9 @@ class Hexapod:
         self.leftturn_path = gen_leftturn_path()
         self.rightturn_path = gen_rightturn_path()
 
+        self.shiftleft_path = gen_shiftleft_path()
+        self.shiftright_path = gen_shiftright_path()
+
         self.standby()
         time.sleep(1)
 
@@ -109,11 +113,17 @@ class Hexapod:
         # for mm in range(0, 20):
         #     self.move(self.fastbackward_path, 0.005)
 
+        # for mm in range(0, 20):
+        #     self.move(self.leftturn_path, 0.005)
+
+        # for mm in range(0, 20):
+        #     self.move(self.rightturn_path, 0.005)
+
         for mm in range(0, 20):
-            self.move(self.leftturn_path, 0.005)
+            self.move(self.shiftleft_path, 0.005)
 
         for mm in range(0, 20):
-            self.move(self.rightturn_path, 0.005)
+            self.move(self.shiftright_path, 0.005)
 
         time.sleep(1)
         self.standby()
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index 6396085..25e8222 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -132,3 +132,47 @@ def gen_rightturn_path():
     rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180))
 
     return rightturn
+
+
+def gen_shiftleft_path():
+    g_steps = 20
+    g_radius = 25
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    # shift 90 degree to make the path "left" shift
+    path = path_rotate_z(path, 90)
+    mir_path = np.roll(path, halfsteps, axis=0)
+
+    shiftleft = np.zeros((g_steps, 6, 3))
+    shiftleft[:, 0, :] = path
+    shiftleft[:, 1, :] = mir_path
+    shiftleft[:, 2, :] = path
+    shiftleft[:, 3, :] = mir_path
+    shiftleft[:, 4, :] = path
+    shiftleft[:, 5, :] = mir_path
+
+    return shiftleft
+
+
+def gen_shiftright_path():
+    g_steps = 20
+    g_radius = 25
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    # shift 90 degree to make the path "left" shift
+    path = path_rotate_z(path, 270)
+    mir_path = np.roll(path, halfsteps, axis=0)
+
+    shiftright = np.zeros((g_steps, 6, 3))
+    shiftright[:, 0, :] = path
+    shiftright[:, 1, :] = mir_path
+    shiftright[:, 2, :] = path
+    shiftright[:, 3, :] = mir_path
+    shiftright[:, 4, :] = path
+    shiftright[:, 5, :] = mir_path
+
+    return shiftright