From 4e0b8bd27b8ff562e858c3b7f6b40a07d01121a5 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng 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