From 5c26cd09ca8a21817498fd42039fbcafe70140a2 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 09:40:50 -0500
Subject: [PATCH] add backward move

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

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index fc2c29f..a4d0852 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -11,7 +11,7 @@ from leg import Leg
 import numpy as np
 import time
 import json
-from path_generator import forward_path
+from path_generator import forward_path, backward_path
 
 
 SIN30 = 0.5
@@ -93,6 +93,11 @@ class Hexapod:
         for mm in range(0, 30):
             self.move(full_path, 0.005)
 
+        full_path = backward_path()
+
+        for mm in range(0, 30):
+            self.move(full_path, 0.005)
+
     def move(self, path, interval):
         for p_idx in range(0, np.shape(path)[0]):
             dest = path[p_idx, :, :]+self.standby_coordinate
diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py
index 93156fc..533c998 100644
--- a/software/rpi/path_generator.py
+++ b/software/rpi/path_generator.py
@@ -20,3 +20,23 @@ def forward_path():
     path[:, 5, :] = mir_path
 
     return path
+
+
+def backward_path():
+    # assert (g_steps % 4) == 0
+    g_steps = 20
+    g_radius = 25
+    halfsteps = int(g_steps/2)
+
+    path = np.zeros((g_steps, 6, 3))
+
+    path[:, 0, :] = 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
+
+    return path