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

Update hexapod.py

Browse Source
This commit is contained in:
Zhengyu Peng 2022-02-16 20:34:49 -05:00
parent af52445efa
commit bfc1fedbde
1 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
software/raspberry pi/hexapod.py
View File

@ -124,6 +124,7 @@ class Hexapod(Thread):
# self.leg_5.reset(True) # self.leg_5.reset(True)
self.standby_coordinate = self.calculate_standby_coordinate(60, 75) self.standby_coordinate = self.calculate_standby_coordinate(60, 75)
self.laydown_coordinate = self.calculate_laydown_coordinate(self.standby_coordinate)
self.forward_path = gen_forward_path(self.standby_coordinate) self.forward_path = gen_forward_path(self.standby_coordinate)
self.backward_path = gen_backward_path(self.standby_coordinate) self.backward_path = gen_backward_path(self.standby_coordinate)
self.fastforward_path = gen_fastforward_path(self.standby_coordinate) self.fastforward_path = gen_fastforward_path(self.standby_coordinate)
@ -144,7 +145,8 @@ class Hexapod(Thread):
self.current_motion = None self.current_motion = None
self.standby() # self.standby()
self.laydown()
time.sleep(1) time.sleep(1)
# self.leg_0.set_angle(1, 30) # self.leg_0.set_angle(1, 30)
@ -194,10 +196,10 @@ class Hexapod(Thread):
np.sin(j3_rad) np.sin(j3_rad)
return standby_coordinate return standby_coordinate
def calculate_sit_coordinate(self, standby_coordinate): def calculate_laydown_coordinate(self, standby_coordinate):
sit_coordinate = np.zeros_like(standby_coordinate) laydown_coordinate = np.zeros_like(standby_coordinate)
sit_coordinate[:, 0:2] = standby_coordinate[:, 0:2] laydown_coordinate[:, 0:2] = standby_coordinate[:, 0:2]
return sit_coordinate return laydown_coordinate
def move(self, path): def move(self, path):
for p_idx in range(0, np.shape(path)[0]): for p_idx in range(0, np.shape(path)[0]):
@ -284,6 +286,19 @@ class Hexapod(Thread):
self.leg_2.move_junctions(angles[2, :]) self.leg_2.move_junctions(angles[2, :])
self.leg_3.move_junctions(angles[3, :]) self.leg_3.move_junctions(angles[3, :])
def laydown(self):
dest = self.laydown_coordinate
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, :])
def inverse_kinematics(self, dest): def inverse_kinematics(self, dest):
temp_dest = dest-self.mount_position temp_dest = dest-self.mount_position
local_dest = np.zeros_like(dest) local_dest = np.zeros_like(dest)