From 44aac110261a8145b84bc9e905f3f0f889cf762c Mon Sep 17 00:00:00 2001 From: Zhengyu Peng 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