climb
This commit is contained in:
parent
ebeb098713
commit
c6b9405b27
@ -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()
|
||||
|
@ -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
|
||||
|
Loading…
x
Reference in New Issue
Block a user