From c6b9405b272e29e5003699f93a389e9664a3ec03 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 22:56:58 -0500
Subject: [PATCH] climb

---
 software/rpi/hexapod.py        | 12 +++++++++---
 software/rpi/path_generator.py | 34 ++++++++++++++++++++++++++++++++++
 2 files changed, 43 insertions(+), 3 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index 26bbe23..6e856a8 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -15,6 +15,7 @@ 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
+from path_generator import gen_climb_path
 
 
 SIN30 = 0.5
@@ -98,6 +99,8 @@ class Hexapod:
         self.shiftleft_path = gen_shiftleft_path()
         self.shiftright_path = gen_shiftright_path()
 
+        self.climb_path = gen_climb_path()
+
         self.standby()
         time.sleep(1)
 
@@ -119,11 +122,14 @@ class Hexapod:
         # for mm in range(0, 20):
         #     self.move(self.rightturn_path, 0.005)
 
-        for mm in range(0, 20):
-            self.move(self.shiftleft_path, 0.005)
+        # for mm in range(0, 20):
+        #     self.move(self.shiftleft_path, 0.005)
+
+        # for mm in range(0, 20):
+        #     self.move(self.shiftright_path, 0.005)
 
         for mm in range(0, 20):
-            self.move(self.shiftright_path, 0.005)
+            self.move(self.climb_path, 0.005)
 
         time.sleep(1)
         self.standby()
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index 25e8222..42a7908 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -176,3 +176,37 @@ def gen_shiftright_path():
     shiftright[:, 5, :] = mir_path
 
     return shiftright
+
+
+def gen_climb_path():
+    g_steps = 20
+    y_radius = 20
+    z_radius = 80
+    x_radius = 30
+
+    z_shift = -30
+
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius)
+    rpath[:, 2] = rpath[:, 2]+z_shift
+    # rpath = [(x, y, z + z_shift) for x, y,
+    #          z in semicircle2_generator(g_steps, y_radius, z_radius, x_radius)]
+    lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)
+    lpath[:, 2] = lpath[:, 2]+z_shift
+    # lpath = [(x, y, z + z_shift) for x, y,
+    #          z in semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)]
+
+    mir_rpath = np.roll(rpath, halfsteps, axis=0)
+    mir_lpath = np.roll(lpath, halfsteps, axis=0)
+
+    climbpath = np.zeros((g_steps, 6, 3))
+    climbpath[:, 0, :] = rpath
+    climbpath[:, 1, :] = mir_rpath
+    climbpath[:, 2, :] = rpath
+    climbpath[:, 3, :] = mir_lpath
+    climbpath[:, 4, :] = lpath
+    climbpath[:, 5, :] = mir_lpath
+
+    return climbpath