Merge branch 'master' of github.com:rookiepeng/hexapod

README.md

@@ -1,7 +1,7 @@
-![banner](files/hexapod_og.jpg)
-
 # Hexapod
 
+![banner](files/hexapod_og.jpg)
+
 Hexapod using Raspberry Pi Zero W
 
 This is a Hexapod project.
@@ -37,4 +37,3 @@ If you want to make one hexapod by yourself. You should at least knows how to:
 1. [Mechanism](mechanism/) - How to build the body
 1. [Electronics](electronics/) - Circuit explanition
 1. [Software](software/) - The software running on Linkit 7697
-

software/rpi/path_generator.py

@@ -317,8 +317,8 @@ def gen_rotatez_path(standby_coordinate,
         x = xy_radius * np.cos(i*step_angle)
         y = xy_radius * np.sin(i*step_angle)
 
-        m = get_rotate_y_matrix(np.arctan2(
+        m = get_rotate_y_matrix(np.arctan2(x, z_lift)*180/np.pi) * \
-            x, z_lift)*180/np.pi) * get_rotate_x_matrix(np.arctan2(y, z_lift)*180/np.pi)
+            get_rotate_x_matrix(np.arctan2(y, z_lift)*180/np.pi)
 
         path[i, :, :] = ((np.matmul(m, scx.T)).T)[:, :-1]