From 44aac110261a8145b84bc9e905f3f0f889cf762c Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookie.dev@gmail.com>
Date: Fri, 26 Nov 2021 22:15:07 -0500
Subject: [PATCH] add Leg and Hexapod classes

---
 software/rpi/__init__.py |  0
 software/rpi/hexapod.py  | 88 +++++++++++++++++++++++-----------------
 software/rpi/leg.py      | 17 ++++++++
 3 files changed, 67 insertions(+), 38 deletions(-)
 create mode 100644 software/rpi/__init__.py
 create mode 100644 software/rpi/leg.py

diff --git a/software/rpi/__init__.py b/software/rpi/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index 4dda7a7..5273cac 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -5,52 +5,64 @@
 # https://circuitpython.readthedocs.io/projects/servokit/en/latest/
 from adafruit_servokit import ServoKit
 
-import time
-
-# Constants
-nbPCAServo = 16
-
-
-# Objects
-pca1 = ServoKit(channels=16, address=0x40, frequency=120)
-pca2 = ServoKit(channels=16, address=0x41, frequency=120)
-
-# function init
+from leg import Leg
 
+import time
 
-def init():
-    print('Init')
-
-    # for i in range(nbPCAServo):
-    #     pca1.servo[i].set_pulse_width_range(MIN_IMP[i] , MAX_IMP[i])
-    #     pca2.servo[i].set_pulse_width_range(MIN_IMP[i] , MAX_IMP[i])
 
-# function main
+class Hexapod:
+
+    def __init__(self):
+        # Objects
+        self.pca_right = ServoKit(channels=16, address=0x40, frequency=120)
+        self.pca_left = ServoKit(channels=16, address=0x41, frequency=120)
+
+        # front right
+        self.leg_0 = Leg('fr',
+                         [self.pca_right.servo[15], self.pca_right.servo[1],
+                             self.pca_right.servo[2]],
+                         correction=[0, 0, 0],
+                         constraint=[0, 180])
+        # center right
+        self.leg_1 = Leg('cr',
+                         [self.pca_right.servo[7], self.pca_right.servo[6],
+                             self.pca_right.servo[8]],
+                         correction=[0, 0, 0],
+                         constraint=[0, 180])
+        # rear right
+        self.leg_2 = Leg('rr',
+                         [self.pca_right.servo[0], self.pca_right.servo[13],
+                             self.pca_right.servo[14]],
+                         correction=[0, 0, 0],
+                         constraint=[0, 180])
+        # rear left
+        self.leg_3 = Leg('rl',
+                         [self.pca_left.servo[15], self.pca_left.servo[2],
+                             self.pca_left.servo[1]],
+                         correction=[0, 0, 0],
+                         constraint=[0, 180])
+        # center left
+        self.leg_4 = Leg('cl',
+                         [self.pca_left.servo[7], self.pca_left.servo[8],
+                             self.pca_left.servo[6]],
+                         correction=[0, 0, 0],
+                         constraint=[0, 180])
+        # front left
+        self.leg_5 = Leg('fl',
+                         [self.pca_left.servo[0], self.pca_left.servo[14],
+                             self.pca_left.servo[13]],
+                         correction=[0, 0, 0],
+                         constraint=[0, 180])
+
+        self.leg_5.set_angle(0, 45)
+        self.leg_5.set_angle(1, 45)
+        self.leg_5.set_angle(2, 45)
 
 
 def main():
 
-    pcaScenario()
-
-
-# function pcaScenario
-def pcaScenario():
-    """Scenario to test servo"""
-    for i in range(nbPCAServo):
-        pca1.servo[i].angle = 90
-        pca2.servo[i].angle = 90
-        # for j in range(MIN_ANG[i], MAX_ANG[i], 1):
-        #     print("Send angle {} to Servo {}".format(j, i))
-        #     pca.servo[i].angle = j
-        #     time.sleep(0.01)
-        # for j in range(MAX_ANG[i], MIN_ANG[i], -1):
-        #     print("Send angle {} to Servo {}".format(j, i))
-        #     pca.servo[i].angle = j
-        #     time.sleep(0.01)
-        # pca.servo[i].angle = None  # disable channel
-    time.sleep(0.5)
+    hexapod = Hexapod()
 
 
 if __name__ == '__main__':
-    init()
     main()
diff --git a/software/rpi/leg.py b/software/rpi/leg.py
new file mode 100644
index 0000000..e15bad2
--- /dev/null
+++ b/software/rpi/leg.py
@@ -0,0 +1,17 @@
+# -*- coding: utf-8 -*-
+
+class Leg:
+    def __init__(self,
+                 id,
+                 junction_servos,
+                 correction=[0, 0, 0],
+                 constraint=[0, 180]):
+        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])
+        self.junction_servos[junction].angle = set_angle