From 5eb582b24be5f44db9992510bcfa006eca43e476 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Thu, 9 Dec 2021 21:54:12 -0500 Subject: [PATCH] add left turn --- software/rpi/hexapod.py | 24 ++++++++++++++---------- software/rpi/path_generator.py | 22 ++++++++++++++++++++++ 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 058bcf3..a29fb61 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -13,6 +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 SIN30 = 0.5 @@ -90,20 +91,25 @@ class Hexapod: self.fastforward_path = gen_fastforward_path() self.fastbackward_path = gen_fastbackward_path() + self.leftturn_path = gen_leftturn_path() + self.standby() time.sleep(1) - for mm in range(0, 20): - self.move(self.forward_path, 0.005) + # 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) + + # for mm in range(0, 20): + # self.move(self.fastbackward_path, 0.005) for mm in range(0, 20): - self.move(self.fastforward_path, 0.005) - - for mm in range(0, 20): - self.move(self.fastbackward_path, 0.005) + self.move(self.leftturn_path, 0.005) time.sleep(1) self.standby() @@ -183,8 +189,6 @@ class Hexapod: return angles - # return [path, mir_path, path, mir_path, path, mir_path, ], "shift", 20, (0, halfsteps) - def main(): diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 9f7d63e..e940246 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -1,5 +1,7 @@ from lib import semicircle_generator, semicircle2_generator +from lib import path_rotate_z import numpy as np +from collections import deque def gen_forward_path(): @@ -90,3 +92,23 @@ def gen_fastbackward_path(): path[:, 5, :] = mir_lpath return path + + +def gen_leftturn_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) + + leftturn = np.zeros((g_steps, 6, 3)) + leftturn[:, 0, :] = np.array(path_rotate_z(path, 45)) + leftturn[:, 1, :] = np.array(path_rotate_z(mir_path, 0)) + leftturn[:, 2, :] = np.array(path_rotate_z(path, 315)) + leftturn[:, 3, :] = np.array(path_rotate_z(mir_path, 225)) + leftturn[:, 4, :] = np.array(path_rotate_z(path, 180)) + leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135)) + + return leftturn