add left turn
This commit is contained in:
parent
2d105530ae
commit
5eb582b24b
@ -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.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.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)
|
||||
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():
|
||||
|
||||
|
@ -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
|
||||
|
Loading…
x
Reference in New Issue
Block a user