From 5eb582b24be5f44db9992510bcfa006eca43e476 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 21:54:12 -0500
Subject: [PATCH] add left turn

---
 software/rpi/hexapod.py        | 24 ++++++++++++++----------
 software/rpi/path_generator.py | 22 ++++++++++++++++++++++
 2 files changed, 36 insertions(+), 10 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index 058bcf3..a29fb61 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -13,6 +13,7 @@ import time
 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
 
 
 SIN30 = 0.5
@@ -90,20 +91,25 @@ class Hexapod:
         self.fastforward_path = gen_fastforward_path()
         self.fastbackward_path = gen_fastbackward_path()
 
+        self.leftturn_path = gen_leftturn_path()
+
         self.standby()
         time.sleep(1)
 
-        for mm in range(0, 20):
-            self.move(self.forward_path, 0.005)
+        # 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)
+
+        # for mm in range(0, 20):
+        #     self.move(self.fastbackward_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)
+            self.move(self.leftturn_path, 0.005)
 
         time.sleep(1)
         self.standby()
@@ -183,8 +189,6 @@ class Hexapod:
 
         return angles
 
-        # return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps)
-
 
 def main():
 
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index 9f7d63e..e940246 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -1,5 +1,7 @@
 from lib import semicircle_generator, semicircle2_generator
+from lib import path_rotate_z
 import numpy as np
+from collections import deque
 
 
 def gen_forward_path():
@@ -90,3 +92,23 @@ def gen_fastbackward_path():
     path[:, 5, :] = mir_lpath
 
     return path
+
+
+def gen_leftturn_path():
+    g_steps = 20
+    g_radius = 25
+    assert (g_steps % 4) == 0
+    halfsteps = int(g_steps/2)
+
+    path = semicircle_generator(g_radius, g_steps)
+    mir_path = np.roll(path, halfsteps, axis=0)
+
+    leftturn = np.zeros((g_steps, 6, 3))
+    leftturn[:, 0, :] = np.array(path_rotate_z(path, 45))
+    leftturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0))
+    leftturn[:, 2, :] = np.array(path_rotate_z(path, 315))
+    leftturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225))
+    leftturn[:, 4, :] = np.array(path_rotate_z(path, 180))
+    leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135))
+
+    return leftturn