update

software/rpi/config.json

@@ -2,9 +2,6 @@
     "legNames":["front_right", "center_right", "rear_right", "rear_left", "center_left", "front_left"],
     "legMountX":[29.41, 36.87, 29.41, -29.41, -36.87, -29.41],
     "legMountY":[55.41, 0, -55.41, -55.41, 0, 55.41],
-    "legMountLeftRightX":36.87,
-    "legMountOtherX":29.41,
-    "legMountOtherY":55.41,
     "legRootToJoint1":20.75,
     "legJoint1ToJoint2":28.0,
     "legJoint2ToJoint3":42.6,

software/rpi/hexapod.py

@@ -23,10 +23,28 @@ COS15 = 0.9659
 class Hexapod:
 
     def __init__(self):
+        # x -> right
+        # y -> front
+        # z -> up
+        # origin is the center of the body
+        # roots are the positions of the bottom screws
+        # length units are in mm
+        # time units are in ms
 
         with open('./config.json', 'r') as read_file:
             self.config = json.load(read_file)
 
+        self.mount_x = np.array(self.config['legMountX'])
+        self.mount_y = np.array(self.config['legMountY'])
+        self.root_j1 = self.config['legRootToJoint1']
+        self.j1_j2 = self.config['legJoint1ToJoint2']
+        self.j2_j3 = self.config['legJoint2ToJoint3']
+        self.j3_tip = self.config['legJoint3ToTip']
+
+        self.mount_position = np.zeros((6, 3))
+        self.mount_position[:, 0] = self.mount_x
+        self.mount_position[:, 1] = self.mount_y
+
         # Objects
         self.pca_right = ServoKit(channels=16, address=0x40, frequency=120)
         self.pca_left = ServoKit(channels=16, address=0x41, frequency=120)
@@ -69,13 +87,17 @@ class Hexapod:
         # self.leg_4.reset()
         # self.leg_5.reset()
 
-        self.mount_position = np.zeros((6, 3))
-        self.mount_position[:, 0] = np.array(self.config['legMountX'])
-        self.mount_position[:, 1] = np.array(self.config['legMountY'])
-
         self.standby()
 
     def standby(self):
+        self.standby_coordinate = np.zeros((6, 3))
+        self.standby_coordinate[:, 2] = self.j2_j3 * \
+            SIN30 - self.j3_tip * COS15
+        self.standby_coordinate[:, 0] = np.array(self.mount_x)+(self.root_j1+self.j1_j2+(
+            self.j2_j3*COS30)+self.j3_tip*SIN15)*1
+        self.standby_coordinate[:, 1] = self.mount_y + (self.root_j1+self.j1_j2+(
+            self.j2_j3*COS30)+self.j3_tip*SIN15)*0
+
         self.leg_0.set_angle(0, 90)
         self.leg_0.set_angle(1, 60)
         self.leg_0.set_angle(2, 75)
@@ -99,33 +121,26 @@ class Hexapod:
         self.leg_5.set_angle(0, 90)
         self.leg_5.set_angle(1, 60)
         self.leg_5.set_angle(2, 75)
-        # self.standby_coordinate = np.zeros((6, 3))
-        # self.standby_coordinate[:, 2] = self.config['legJoint2ToJoint3'] * \
-        #     SIN30 - self.config['legJoint3ToTip'] * COS15
-        # self.standby_coordinate[:, 0] = np.array(self.config['legMountX'])+(self.config['legRootToJoint1']+self.config['legJoint1ToJoint2']+(
-        #     self.config['legJoint2ToJoint3']*COS30)+self.config['legJoint3ToTip']*SIN15)*1
-        # self.standby_coordinate[:, 1] = np.array(self.config['legMountY']) + (self.config['legRootToJoint1']+self.config['legJoint1ToJoint2']+(
-        #     self.config['legJoint2ToJoint3']*COS30)+self.config['legJoint3ToTip']*SIN15)*0
 
-        # self.ik(self.standby_coordinate)
 
     def ik(self, to):
         to = to-self.mount_position
+
         self.angles = np.zeros((6, 3))
-        x = to[:, 0] - self.config['legRootToJoint1']
+        x = to[:, 0] - self.root_j1
         y = to[:, 1]
 
         self.angles[:, 0] = (np.arctan2(y, x) * 180 / np.pi)
 
-        x = np.sqrt(x*x + y*y) - self.config['legJoint1ToJoint2']
+        x = np.sqrt(x*x + y*y) - self.j1_j2
         y = to[:, 2]
         ar = np.arctan2(y, x)
         lr2 = x*x + y*y
         lr = np.sqrt(lr2)
-        a1 = np.arccos((lr2 + self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] -
-                        self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint2ToJoint3']*lr))
-        a2 = np.arccos((lr2 - self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] +
-                        self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint3ToTip']*lr))
+        a1 = np.arccos((lr2 + self.j2_j3*self.j2_j3 -
+                        self.j3_tip*self.j3_tip)/(2*self.j2_j3*lr))
+        a2 = np.arccos((lr2 - self.j2_j3*self.j2_j3 +
+                        self.j3_tip*self.j3_tip)/(2*self.j3_tip*lr))
 
         self.angles[:, 1] = ((ar + a1) * 180 / np.pi)
         self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))