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

add left turn

Browse Source
This commit is contained in:
Zhengyu Peng
2021-12-09 21:54:12 -05:00
parent 118769b2c5
commit 93d0994725
2 changed files with 37 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
software/rpi/path_generator.py
View File

@ -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