diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index a3567f9..06ad0cb 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -62,7 +62,7 @@ class Hexapod: self.leg_0 = Leg(0, [self.pca_left.servo[15], self.pca_left.servo[2], self.pca_left.servo[1]], - correction=[-6, 4, -6]) + correction=[-6, 4, -4]) # center right self.leg_1 = Leg(1, [self.pca_left.servo[7], self.pca_left.servo[8], @@ -117,30 +117,30 @@ class Hexapod: 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.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.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) - # 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) - # for mm in range(0, 20): - # self.move(self.climb_path, 0.005) + for mm in range(0, 20): + self.move(self.climb_path, 0.005) # self.move(self.rotatex_path, 0.005) - for mm in range(0, 20): - self.move(self.twist_path, 0.005) + # for mm in range(0, 20): + # self.move(self.rotatez_path, 0.005) time.sleep(1) self.standby() @@ -161,7 +161,6 @@ class Hexapod: def move(self, path, interval): for p_idx in range(0, np.shape(path)[0]): - # dest = path[p_idx, :, :]+self.standby_coordinate dest = path[p_idx, :, :] angles = self.inverse_kinematics(dest) diff --git a/software/rpi/lib.py b/software/rpi/lib.py index 9c8bcd8..eb45b0b 100644 --- a/software/rpi/lib.py +++ b/software/rpi/lib.py @@ -106,4 +106,4 @@ def path_rotate_z(path, angle): if __name__ == '__main__': pt = [0, 1, 0] - print(point_rotate_z(pt, 270)) + diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 2fc1a19..4567a70 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -110,102 +110,89 @@ def gen_leftturn_path(standby_coordinate, return path+np.tile(standby_coordinate, (g_steps, 1, 1)) -def gen_rightturn_path(standby_coordinate): - g_steps = 20 - g_radius = 25 +def gen_rightturn_path(standby_coordinate, + 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) + semi_circle = semicircle_generator(g_radius, g_steps) + mir_path = np.roll(semi_circle, halfsteps, axis=0) - rightturn = np.zeros((g_steps, 6, 3)) - rightturn[:, 0, :] = path_rotate_z(path, 45+180) - rightturn[:, 1, :] = path_rotate_z(mir_path, 0+180) - rightturn[:, 2, :] = path_rotate_z(path, 315+180) - rightturn[:, 3, :] = path_rotate_z(mir_path, 225+180) - rightturn[:, 4, :] = path_rotate_z(path, 180+180) - rightturn[:, 5, :] = path_rotate_z(mir_path, 135+180) + path = np.zeros((g_steps, 6, 3)) + path[:, 0, :] = path_rotate_z(semi_circle, 45+180) + path[:, 1, :] = path_rotate_z(mir_path, 0+180) + path[:, 2, :] = path_rotate_z(semi_circle, 315+180) + path[:, 3, :] = path_rotate_z(mir_path, 225+180) + path[:, 4, :] = path_rotate_z(semi_circle, 180+180) + path[:, 5, :] = path_rotate_z(mir_path, 135+180) - return rightturn+np.tile(standby_coordinate, (g_steps, 1, 1)) + return path+np.tile(standby_coordinate, (g_steps, 1, 1)) -def gen_shiftleft_path(standby_coordinate): - g_steps = 20 - g_radius = 25 +def gen_shiftleft_path(standby_coordinate, + g_steps = 20, + g_radius = 25): assert (g_steps % 4) == 0 halfsteps = int(g_steps/2) - path = semicircle_generator(g_radius, g_steps) + semi_circle = 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) + semi_circle = np.array(path_rotate_z(semi_circle, 90)) + mir_path = np.roll(semi_circle, 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 + path = np.zeros((g_steps, 6, 3)) + path[:,[0,2,4],:] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1)) + path[:,[1,3,5],:] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1)) - return shiftleft+np.tile(standby_coordinate, (g_steps, 1, 1)) + return path+np.tile(standby_coordinate, (g_steps, 1, 1)) -def gen_shiftright_path(standby_coordinate): - g_steps = 20 - g_radius = 25 +def gen_shiftright_path(standby_coordinate, + g_steps = 20, + g_radius = 25): assert (g_steps % 4) == 0 halfsteps = int(g_steps/2) - path = semicircle_generator(g_radius, g_steps) + semi_circle = 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+np.tile(standby_coordinate, (g_steps, 1, 1)) + semi_circle = np.array(path_rotate_z(semi_circle, 270)) + mir_path = np.roll(semi_circle, halfsteps, axis=0) + path = np.zeros((g_steps, 6, 3)) + path[:,[0,2,4],:] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1)) + path[:,[1,3,5],:] = np.tile(mir_path[:, np.newaxis, :], (1, 3, 1)) -def gen_climb_path(standby_coordinate): - g_steps = 20 - y_radius = 20 - z_radius = 80 - x_radius = 30 + return path+np.tile(standby_coordinate, (g_steps, 1, 1)) - z_shift = -30 +def gen_climb_path(standby_coordinate, + 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 + path = np.zeros((g_steps, 6, 3)) + path[:, 0, :] = rpath + path[:, 1, :] = mir_rpath + path[:, 2, :] = rpath + path[:, 3, :] = mir_lpath + path[:, 4, :] = lpath + path[:, 5, :] = mir_lpath - return climbpath+np.tile(standby_coordinate, (g_steps, 1, 1)) + return path+np.tile(standby_coordinate, (g_steps, 1, 1)) def gen_rotatex_path(standby_coordinate):