From 9a2aa191aebd4ef979b813f99a529295b999894e Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Thu, 9 Dec 2021 20:51:04 -0500
Subject: [PATCH] increase limitation

---
 software/rpi/hexapod.py | 14 +++++++++-----
 software/rpi/leg.py     | 11 +++++++----
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index a4d0852..f2e5ac3 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -88,10 +88,12 @@ class Hexapod:
         self.standby()
         time.sleep(0.1)
 
-        full_path = forward_path()
+        # self.leg_0.set_angle(1, 45)
 
-        for mm in range(0, 30):
-            self.move(full_path, 0.005)
+        # full_path = forward_path()
+
+        # for mm in range(0, 30):
+        #     self.move(full_path, 0.005)
 
         full_path = backward_path()
 
@@ -104,11 +106,13 @@ class Hexapod:
             angles = self.inverse_kinematics(dest)
 
             self.leg_0.move_junctions(angles[0, :])
+            self.leg_5.move_junctions(angles[5, :])
+
             self.leg_1.move_junctions(angles[1, :])
+            self.leg_4.move_junctions(angles[4, :])
+
             self.leg_2.move_junctions(angles[2, :])
             self.leg_3.move_junctions(angles[3, :])
-            self.leg_4.move_junctions(angles[4, :])
-            self.leg_5.move_junctions(angles[5, :])
 
             time.sleep(interval)
 
diff --git a/software/rpi/leg.py b/software/rpi/leg.py
index 87372f6..1096854 100644
--- a/software/rpi/leg.py
+++ b/software/rpi/leg.py
@@ -1,20 +1,23 @@
 # -*- coding: utf-8 -*-
+import numpy as np
+
 
 class Leg:
     def __init__(self,
                  id,
                  junction_servos,
                  correction=[0, 0, 0],
-                 constraint=[[45, 135], [45, 165], [30, 150]]):
+                 constraint=[[35, 145], [30, 165], [30, 150]]):
         self.id = id
         self.junction_servos = junction_servos
         self.correction = correction
         self.constraint = constraint
 
     def set_angle(self, junction, angle):
-        set_angle = min(
-            angle+self.correction[junction], self.constraint[junction][1])
-        set_angle = max(set_angle, self.constraint[junction][0])
+        set_angle = np.min(
+            [angle+self.correction[junction], self.constraint[junction][1]+self.correction[junction], 180])
+        set_angle = np.max(
+            [set_angle, self.constraint[junction][0]+self.correction[junction], 0])
         self.junction_servos[junction].angle = set_angle
 
     def move_junctions(self, angles):