From d7b347c9221574d398bf5cd235bb1146b48e5bfa Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Wed, 8 Dec 2021 13:00:13 -0500
Subject: [PATCH] update

---
 software/rpi/hexapod.py | 70 ++++++++++++++++++++++++++++-------------
 1 file changed, 49 insertions(+), 21 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index b07653d..86c034e 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -62,42 +62,70 @@ class Hexapod:
                              self.pca_right.servo[14]],
                          correction=[-6, 4, 0])
 
-        self.leg_0.reset()
-        self.leg_1.reset()
-        self.leg_2.reset()
-        self.leg_3.reset()
-        self.leg_4.reset()
-        self.leg_5.reset()
+        # self.leg_0.reset()
+        # self.leg_1.reset()
+        # self.leg_2.reset()
+        # self.leg_3.reset()
+        # 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.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)*COS45
-        self.standby_coordinate[:, 1] = np.array(self.config['legMountY']) + (self.config['legRootToJoint1']+self.config['legJoint1ToJoint2']+(
-            self.config['legJoint2ToJoint3']*COS30)+self.config['legJoint3ToTip']*SIN15)*SIN45
-
-        self.ik(self.standby_coordinate)
+        self.leg_0.set_angle(0, 90)
+        self.leg_0.set_angle(1, 60)
+        self.leg_0.set_angle(2, 75)
+
+        self.leg_1.set_angle(0, 90)
+        self.leg_1.set_angle(1, 60)
+        self.leg_1.set_angle(2, 75)
+
+        self.leg_2.set_angle(0, 90)
+        self.leg_2.set_angle(1, 60)
+        self.leg_2.set_angle(2, 75)
+
+        self.leg_3.set_angle(0, 90)
+        self.leg_3.set_angle(1, 60)
+        self.leg_3.set_angle(2, 75)
+
+        self.leg_4.set_angle(0, 90)
+        self.leg_4.set_angle(1, 60)
+        self.leg_4.set_angle(2, 75)
+
+        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']
         y = to[:, 1]
 
-        self.angles[:, 0] = (np.atan2(y, x) * 180 / np.pi)
+        self.angles[:, 0] = (np.arctan2(y, x) * 180 / np.pi)
 
         x = np.sqrt(x*x + y*y) - self.config['legJoint1ToJoint2']
         y = to[:, 2]
-        ar = np.atan2(y, x)
+        ar = np.arctan2(y, x)
         lr2 = x*x + y*y
         lr = np.sqrt(lr2)
-        a1 = np.acos((lr2 + self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] -
-                      self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint2ToJoint3']*lr))
-        a2 = np.acos((lr2 - self.config['legJoint2ToJoint3']*self.config['legJoint2ToJoint3'] +
-                      self.config['legJoint3ToTip']*self.config['legJoint3ToTip'])/(2*self.config['legJoint3ToTip']*lr))
+        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))
 
         self.angles[:, 1] = ((ar + a1) * 180 / np.pi)
         self.angles[:, 2] = (90 - ((a1 + a2) * 180 / np.pi))