From 5fa1b2f048cf254eeabd41fcc6d90d8ccfbeadc7 Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Tue, 7 Dec 2021 21:31:09 -0500
Subject: [PATCH] calibrate legs

---
 software/rpi/hexapod.py | 18 ++++++------------
 software/rpi/leg.py     |  7 ++++---
 2 files changed, 10 insertions(+), 15 deletions(-)

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index a1a1dfa..1505b49 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -21,38 +21,32 @@ class Hexapod:
         self.leg_0 = Leg('rr',
                          [self.pca_left.servo[15], self.pca_left.servo[2],
                              self.pca_left.servo[1]],
-                         correction=[0, 0, 0],
-                         constraint=[0, 180])
+                         correction=[-6, 4, -6])
         # center right
         self.leg_1 = Leg('cr',
                          [self.pca_left.servo[7], self.pca_left.servo[8],
                              self.pca_left.servo[6]],
-                         correction=[0, 0, 0],
-                         constraint=[0, 180])
+                         correction=[3, -5, -6])
         # front right
         self.leg_2 = Leg('fr',
                          [self.pca_left.servo[0], self.pca_left.servo[14],
                              self.pca_left.servo[13]],
-                         correction=[0, 0, 0],
-                         constraint=[0, 180])
+                         correction=[3, -6, -5])
         # front left
         self.leg_3 = Leg('fl',
                          [self.pca_right.servo[15], self.pca_right.servo[1],
                              self.pca_right.servo[2]],
-                         correction=[0, 0, 0],
-                         constraint=[0, 180])
+                         correction=[-3, -4, 6])
         # center left
         self.leg_4 = Leg('cl',
                          [self.pca_right.servo[7], self.pca_right.servo[6],
                              self.pca_right.servo[8]],
-                         correction=[0, 0, 0],
-                         constraint=[0, 180])
+                         correction=[-6, 0, 0])
         # rear left
         self.leg_5 = Leg('rl',
                          [self.pca_right.servo[0], self.pca_right.servo[13],
                              self.pca_right.servo[14]],
-                         correction=[0, 0, 0],
-                         constraint=[0, 180])
+                         correction=[-6, 4, 0])
 
         self.leg_0.set_angle(0, 90)
         self.leg_0.set_angle(1, 90)
diff --git a/software/rpi/leg.py b/software/rpi/leg.py
index e15bad2..836f2a2 100644
--- a/software/rpi/leg.py
+++ b/software/rpi/leg.py
@@ -5,13 +5,14 @@ class Leg:
                  id,
                  junction_servos,
                  correction=[0, 0, 0],
-                 constraint=[0, 180]):
+                 constraint=[[45, 135], [45, 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[1])
-        set_angle = max(set_angle, self.constraint[0])
+        set_angle = min(
+            angle+self.correction[junction], self.constraint[junction][1])
+        set_angle = max(set_angle, self.constraint[junction][0])
         self.junction_servos[junction].angle = set_angle