shark/hexapod
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

WIP

Browse Source
This commit is contained in:
Zhengyu Peng 2021-12-08 11:11:50 -05:00
parent 1bdd862442
commit 7aefe1a5ad
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
software/rpi/config.json
View File

@ -1,7 +1,7 @@
{ {
"legNames":["front_right", "center_right", "rear_right", "rear_left", "center_left", "front_left"], "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], "legMountX":[29.41, 36.87, 29.41, -29.41, -36.87, -29.41],
"legMountY":[55.41, 0, 55.41, 55.41, 0, 55.41], "legMountY":[55.41, 0, -55.41, -55.41, 0, 55.41],
"legMountLeftRightX":36.87, "legMountLeftRightX":36.87,
"legMountOtherX":29.41, "legMountOtherX":29.41,
"legMountOtherY":55.41, "legMountOtherY":55.41,

18
software/rpi/hexapod.py
View File

@ -7,9 +7,18 @@ from adafruit_servokit import ServoKit
from leg import Leg from leg import Leg
# python3-numpy
import numpy as np
import time import time
import json import json
SIN30 = 0.5
COS30 = 0.866
SIN45 = 0.7071
COS45 = 0.7071
SIN15 = 0.2588
COS15 = 0.9659
class Hexapod: class Hexapod:
@ -60,6 +69,15 @@ class Hexapod:
self.leg_4.reset() self.leg_4.reset()
self.leg_5.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(): def main():