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