diff --git a/software/raspberry pi/hexapod.py b/software/raspberry pi/hexapod.py index a666d27..cef16a4 100644 --- a/software/raspberry pi/hexapod.py +++ b/software/raspberry pi/hexapod.py @@ -188,9 +188,11 @@ class Hexapod(Thread): posture = np.zeros((6, 3)) posture[:, 0] = self.mount_x+(self.root_j1+self.j1_j2+( - self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad))*np.cos(self.mount_angle) + self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad)) *\ + np.cos(self.mount_angle) posture[:, 1] = self.mount_y + (self.root_j1+self.j1_j2+( - self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad))*np.sin(self.mount_angle) + self.j2_j3*np.sin(j2_rad))+self.j3_tip*np.cos(j3_rad)) *\ + np.sin(self.mount_angle) posture[:, 2] = self.j2_j3 * \ np.cos(j2_rad) - self.j3_tip * \ np.sin(j3_rad)