shift left and right
This commit is contained in:
parent
4e0b8bd27b
commit
ebeb098713
@ -14,6 +14,7 @@ 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, gen_rightturn_path
|
||||
from path_generator import gen_shiftleft_path, gen_shiftright_path
|
||||
|
||||
|
||||
SIN30 = 0.5
|
||||
@ -94,6 +95,9 @@ class Hexapod:
|
||||
self.leftturn_path = gen_leftturn_path()
|
||||
self.rightturn_path = gen_rightturn_path()
|
||||
|
||||
self.shiftleft_path = gen_shiftleft_path()
|
||||
self.shiftright_path = gen_shiftright_path()
|
||||
|
||||
self.standby()
|
||||
time.sleep(1)
|
||||
|
||||
@ -109,11 +113,17 @@ class Hexapod:
|
||||
# for mm in range(0, 20):
|
||||
# self.move(self.fastbackward_path, 0.005)
|
||||
|
||||
for mm in range(0, 20):
|
||||
self.move(self.leftturn_path, 0.005)
|
||||
# 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)
|
||||
|
||||
for mm in range(0, 20):
|
||||
self.move(self.rightturn_path, 0.005)
|
||||
self.move(self.shiftleft_path, 0.005)
|
||||
|
||||
for mm in range(0, 20):
|
||||
self.move(self.shiftright_path, 0.005)
|
||||
|
||||
time.sleep(1)
|
||||
self.standby()
|
||||
|
@ -132,3 +132,47 @@ def gen_rightturn_path():
|
||||
rightturn[:, 5, :] = np.array(path_rotate_z(mir_path, 135+180))
|
||||
|
||||
return rightturn
|
||||
|
||||
|
||||
def gen_shiftleft_path():
|
||||
g_steps = 20
|
||||
g_radius = 25
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
# shift 90 degree to make the path "left" shift
|
||||
path = path_rotate_z(path, 90)
|
||||
mir_path = np.roll(path, halfsteps, axis=0)
|
||||
|
||||
shiftleft = np.zeros((g_steps, 6, 3))
|
||||
shiftleft[:, 0, :] = path
|
||||
shiftleft[:, 1, :] = mir_path
|
||||
shiftleft[:, 2, :] = path
|
||||
shiftleft[:, 3, :] = mir_path
|
||||
shiftleft[:, 4, :] = path
|
||||
shiftleft[:, 5, :] = mir_path
|
||||
|
||||
return shiftleft
|
||||
|
||||
|
||||
def gen_shiftright_path():
|
||||
g_steps = 20
|
||||
g_radius = 25
|
||||
assert (g_steps % 4) == 0
|
||||
halfsteps = int(g_steps/2)
|
||||
|
||||
path = semicircle_generator(g_radius, g_steps)
|
||||
# shift 90 degree to make the path "left" shift
|
||||
path = path_rotate_z(path, 270)
|
||||
mir_path = np.roll(path, halfsteps, axis=0)
|
||||
|
||||
shiftright = np.zeros((g_steps, 6, 3))
|
||||
shiftright[:, 0, :] = path
|
||||
shiftright[:, 1, :] = mir_path
|
||||
shiftright[:, 2, :] = path
|
||||
shiftright[:, 3, :] = mir_path
|
||||
shiftright[:, 4, :] = path
|
||||
shiftright[:, 5, :] = mir_path
|
||||
|
||||
return shiftright
|
||||
|
Loading…
x
Reference in New Issue
Block a user