update

software/rpi/hexapod.py

@@ -114,8 +114,8 @@ class Hexapod:
         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)

software/rpi/lib.py

@@ -17,10 +17,8 @@ def semicircle_generator(radius, steps, reverse=False):
 
     # second half, move forward in semicircle shape (y, z change)
     angle = np.pi - step_angle*halfsteps_array
-    y = radius * np.cos(angle)
-    z = radius * np.sin(angle)
-    result[halfsteps:, 1] = y
-    result[halfsteps:, 2] = z
+    result[halfsteps:, 1] = radius * np.cos(angle)
+    result[halfsteps:, 2] = radius * np.sin(angle)
 
     result = np.roll(result, int(steps/4), axis=0)
 
@@ -43,14 +41,11 @@ def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False):
     # first half, move backward (only y change)
     result[:halfsteps, 1] = y_radius - halfsteps_array*y_radius*2/(halfsteps)
 
-    # second half, move forward in semicircle shape (y, z change)
+    # second half, move forward in semicircle shape (x, y, z change)
     angle = np.pi - step_angle*halfsteps_array
-    y = y_radius * np.cos(angle)
-    z = z_radius * np.sin(angle)
-    x = x_radius * np.sin(angle)
-    result[halfsteps:, 0] = x
-    result[halfsteps:, 1] = y
-    result[halfsteps:, 2] = z
+    result[halfsteps:, 0] = x_radius * np.sin(angle)
+    result[halfsteps:, 1] = y_radius * np.cos(angle)
+    result[halfsteps:, 2] = z_radius * np.sin(angle)
 
     result = np.roll(result, int(steps/4), axis=0)
 

software/rpi/path_generator.py

@@ -5,22 +5,15 @@ import numpy as np
 from collections import deque
 
 
-def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25):
+def gen_forward_path(standby_coordinate, g_steps=20, g_radius=25):
+    assert (g_steps % 4) == 0
+
     halfsteps = int(g_steps/2)
-    g_steps = halfsteps*2
 
     path = np.zeros((g_steps, 6, 3))
 
-    # path[:, 0, :] = semicircle_generator(g_radius, g_steps)
     semi_circle = semicircle_generator(g_radius, g_steps)
 
-    # 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
-
     path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
     path[:, [1, 3, 5], :] = np.tile(
         np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
@@ -28,22 +21,18 @@ def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25):
     return path+np.tile(standby_coordinate, (g_steps, 1, 1))
 
 
-def gen_backward_path(standby_coordinate):
-    # assert (g_steps % 4) == 0
-    g_steps = 20
-    g_radius = 25
+def gen_backward_path(standby_coordinate, g_steps=20, g_radius=25):
+    assert (g_steps % 4) == 0
+
     halfsteps = int(g_steps/2)
 
     path = np.zeros((g_steps, 6, 3))
 
-    path[:, 0, :] = semicircle_generator(g_radius, g_steps, reverse=True)
+    semi_circle = 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
+    path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1))
+    path[:, [1, 3, 5], :] = np.tile(
+        np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1))
 
     return path+np.tile(standby_coordinate, (g_steps, 1, 1))