shark/hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add right turn

Browse Source
This commit is contained in:
Zhengyu Peng 2021-12-09 21:59:32 -05:00
parent 5eb582b24b
commit 4e0b8bd27b
2 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
software/rpi/hexapod.py
View File

@ -13,7 +13,7 @@ import time
import json import json
from path_generator import gen_forward_path, gen_backward_path 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_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 SIN30 = 0.5
@ -92,6 +92,7 @@ class Hexapod:
self.fastbackward_path = gen_fastbackward_path() self.fastbackward_path = gen_fastbackward_path()
self.leftturn_path = gen_leftturn_path() self.leftturn_path = gen_leftturn_path()
self.rightturn_path = gen_rightturn_path()
self.standby() self.standby()
time.sleep(1) time.sleep(1)
@ -111,6 +112,9 @@ class Hexapod:
for mm in range(0, 20): for mm in range(0, 20):
self.move(self.leftturn_path, 0.005) self.move(self.leftturn_path, 0.005)
for mm in range(0, 20):
self.move(self.rightturn_path, 0.005)
time.sleep(1) time.sleep(1)
self.standby() self.standby()

20
software/rpi/path_generator.py
View File

@ -112,3 +112,23 @@ def gen_leftturn_path():
leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135)) leftturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135))
return leftturn 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