From 7aefe1a5ad4c13ebf612af2d31700ce7c5f2cc8b Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookiepeng@users.noreply.github.com>
Date: Wed, 8 Dec 2021 11:11:50 -0500
Subject: [PATCH] WIP

---
 software/rpi/config.json |  4 ++--
 software/rpi/hexapod.py  | 18 ++++++++++++++++++
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/software/rpi/config.json b/software/rpi/config.json
index 94a302a..985d780 100644
--- a/software/rpi/config.json
+++ b/software/rpi/config.json
@@ -1,7 +1,7 @@
 {
     "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],
+    "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,
diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index 8444a80..387e182 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -7,9 +7,18 @@ from adafruit_servokit import ServoKit
 
 from leg import Leg
 
+# python3-numpy
+import numpy as np
 import time
 import json
 
+SIN30 = 0.5
+COS30 = 0.866
+SIN45 = 0.7071
+COS45 = 0.7071
+SIN15 = 0.2588
+COS15 = 0.9659
+
 
 class Hexapod:
 
@@ -60,6 +69,15 @@ class Hexapod:
         self.leg_4.reset()
         self.leg_5.reset()
 
+    def standby(self):
+        self.standby_coordinate = np.zeros((6, 3))
+        self.standby_coordinate[:, 2] = self.config['legJoint3ToTip'] * \
+            COS15 - self.config['legJoint2ToJoint3']*SIN30
+        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
+
 
 def main():