From 4e0b8bd27b8ff562e858c3b7f6b40a07d01121a5 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 21:59:32 -0500
Subject: [PATCH] add right turn

---
 software/rpi/hexapod.py        |  6 +++++-
 software/rpi/path_generator.py | 20 ++++++++++++++++++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index a29fb61..bec076b 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -13,7 +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
+from path_generator import gen_leftturn_path, gen_rightturn_path
 
 
 SIN30 = 0.5
@@ -92,6 +92,7 @@ class Hexapod:
         self.fastbackward_path = gen_fastbackward_path()
 
         self.leftturn_path = gen_leftturn_path()
+        self.rightturn_path = gen_rightturn_path()
 
         self.standby()
         time.sleep(1)
@@ -111,6 +112,9 @@ class Hexapod:
         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)
+
         time.sleep(1)
         self.standby()
 
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index e940246..6396085 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -112,3 +112,23 @@ def gen_leftturn_path():
     leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135))
 
     return leftturn
+
+
+def gen_rightturn_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)
+
+    rightturn = np.zeros((g_steps, 6, 3))
+    rightturn[:, 0, :] = np.array(path_rotate_z(path, 45+180))
+    rightturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0+180))
+    rightturn[:, 2, :] = np.array(path_rotate_z(path, 315+180))
+    rightturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225+180))
+    rightturn[:, 4, :] = np.array(path_rotate_z(path, 180+180))
+    rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180))
+
+    return rightturn