From c6b9405b272e29e5003699f93a389e9664a3ec03 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Thu, 9 Dec 2021 22:56:58 -0500 Subject: [PATCH] climb --- software/rpi/hexapod.py | 12 +++++++++--- software/rpi/path_generator.py | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 26bbe23..6e856a8 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -15,6 +15,7 @@ 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, gen_rightturn_path from path_generator import gen_shiftleft_path, gen_shiftright_path +from path_generator import gen_climb_path SIN30 = 0.5 @@ -98,6 +99,8 @@ class Hexapod: self.shiftleft_path = gen_shiftleft_path() self.shiftright_path = gen_shiftright_path() + self.climb_path = gen_climb_path() + self.standby() time.sleep(1) @@ -119,11 +122,14 @@ class Hexapod: # for mm in range(0, 20): # self.move(self.rightturn_path, 0.005) - for mm in range(0, 20): - self.move(self.shiftleft_path, 0.005) + # for mm in range(0, 20): + # self.move(self.shiftleft_path, 0.005) + + # for mm in range(0, 20): + # self.move(self.shiftright_path, 0.005) for mm in range(0, 20): - self.move(self.shiftright_path, 0.005) + self.move(self.climb_path, 0.005) time.sleep(1) self.standby() diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 25e8222..42a7908 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -176,3 +176,37 @@ def gen_shiftright_path(): shiftright[:, 5, :] = mir_path return shiftright + + +def gen_climb_path(): + g_steps = 20 + y_radius = 20 + z_radius = 80 + x_radius = 30 + + z_shift = -30 + + assert (g_steps % 4) == 0 + halfsteps = int(g_steps/2) + + rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius) + rpath[:, 2] = rpath[:, 2]+z_shift + # rpath = [(x, y, z + z_shift) for x, y, + # z in semicircle2_generator(g_steps, y_radius, z_radius, x_radius)] + lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius) + lpath[:, 2] = lpath[:, 2]+z_shift + # lpath = [(x, y, z + z_shift) for x, y, + # z in semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)] + + mir_rpath = np.roll(rpath, halfsteps, axis=0) + mir_lpath = np.roll(lpath, halfsteps, axis=0) + + climbpath = np.zeros((g_steps, 6, 3)) + climbpath[:, 0, :] = rpath + climbpath[:, 1, :] = mir_rpath + climbpath[:, 2, :] = rpath + climbpath[:, 3, :] = mir_lpath + climbpath[:, 4, :] = lpath + climbpath[:, 5, :] = mir_lpath + + return climbpath