diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 76aa0de..61892b3 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -37,27 +37,22 @@ def gen_backward_path(standby_coordinate, g_steps=20, g_radius=25): return path+np.tile(standby_coordinate, (g_steps, 1, 1)) -def gen_fastforward_path(standby_coordinate): - g_steps = 20 - y_radius = 50 - z_radius = 30 - x_radius = 10 +def gen_fastforward_path(standby_coordinate,g_steps = 20, y_radius = 50, z_radius = 30, x_radius = 10): + assert (g_steps % 2) == 0 halfsteps = int(g_steps/2) path = np.zeros((g_steps, 6, 3)) - path[:, 0, :] = semicircle2_generator( + semi_circle_r = semicircle2_generator( g_steps, y_radius, z_radius, x_radius) - path[:, 4, :] = semicircle2_generator( + semi_circle_l = semicircle2_generator( g_steps, y_radius, z_radius, -x_radius) - mir_rpath = np.roll(path[:, 0, :], halfsteps, axis=0) - path[:, 1, :] = mir_rpath - path[:, 2, :] = path[:, 0, :] - - mir_lpath = np.roll(path[:, 4, :], halfsteps, axis=0) - path[:, 3, :] = mir_lpath - path[:, 5, :] = mir_lpath + path[:, [0,2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1)) + path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0) + path[:, 4, :] = semi_circle_l + path[:, [3,5], :] = np.tile( + np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1)) return path+np.tile(standby_coordinate, (g_steps, 1, 1))