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

Update hexapod.py

Browse Source
master
Zhengyu Peng 3 years ago
parent 7cfb69a276
commit 6705311511
1 changed files with 19 additions and 106 deletions
Show Stats Download Patch File Download Diff File

125
software/raspberry pi/hexapod.py
Unescape Escape View File

@ -148,14 +148,7 @@ class Hexapod(Thread):
self.standby_posture = self.gen_posture(60, 75) self.standby_posture = self.gen_posture(60, 75)
self.climb_path = gen_climb_path(self.standby_posture) self.current_motion = self.standby_posture
self.rotatex_path = gen_rotatex_path(self.standby_posture)
self.rotatey_path = gen_rotatey_path(self.standby_posture)
self.rotatez_path = gen_rotatez_path(self.standby_posture)
self.twist_path = gen_twist_path(self.standby_posture)
self.current_motion = None
self.cmd_dict = { self.cmd_dict = {
self.CMD_STANDBY: self.standby_posture, self.CMD_STANDBY: self.standby_posture,
@ -175,10 +168,18 @@ class Hexapod(Thread):
self.CMD_SHIFTLEFT: gen_shift_path( self.CMD_SHIFTLEFT: gen_shift_path(
self.standby_posture, direction='left'), self.standby_posture, direction='left'),
self.CMD_SHIFTRIGHT: gen_shift_path( self.CMD_SHIFTRIGHT: gen_shift_path(
self.standby_posture, direction='right') self.standby_posture, direction='right'),
self.CMD_CLIMBFORWARD: gen_climb_path(
self.standby_posture, reverse=False),
self.CMD_CLIMBBACKWARD: gen_climb_path(
self.standby_posture, reverse=True),
self.CMD_ROTATEX: gen_rotatex_path(self.standby_posture),
self.CMD_ROTATEY: gen_rotatey_path(self.standby_posture),
self.CMD_ROTATEZ: gen_rotatez_path(self.standby_posture),
self.CMD_TWIST: gen_twist_path(self.standby_posture)
} }
self.standby() self.posture(self.standby_posture)
time.sleep(1) time.sleep(1)
def gen_posture(self, j2_angle, j3_angle): def gen_posture(self, j2_angle, j3_angle):
@ -224,7 +225,7 @@ class Hexapod(Thread):
time.sleep(self.interval) time.sleep(self.interval)
def move_routine(self, path): def motion(self, path):
for p_idx in range(0, np.shape(path)[0]): for p_idx in range(0, np.shape(path)[0]):
dest = path[p_idx, :, :] dest = path[p_idx, :, :]
angles = self.inverse_kinematics(dest) angles = self.inverse_kinematics(dest)
@ -246,67 +247,9 @@ class Hexapod(Thread):
time.sleep(self.interval) time.sleep(self.interval)
pass pass
else: else:
data = cmd_string.split(':')[-2] self.cmd_handler(cmd_string)
if data == 'standby':
self.current_motion = None
self.standby()
elif data == 'forward':
self.current_motion = self.forward_path
elif data == 'backward':
self.current_motion = self.backward_path
elif data == 'fastforward':
self.current_motion = self.fastforward_path
elif data == 'fastbackward':
self.current_motion = self.fastbackward_path
elif data == 'leftturn':
self.current_motion = self.leftturn_path
elif data == 'rightturn':
self.current_motion = self.rightturn_path
elif data == 'shiftleft':
self.current_motion = self.shiftleft_path
elif data == 'shiftright':
self.current_motion = self.shiftright_path
elif data == 'climb':
self.current_motion = self.climb_path
elif data == 'rotatex':
self.current_motion = self.rotatex_path
elif data == 'rotatey':
self.current_motion = self.rotatey_path
elif data == 'rotatez':
self.current_motion = self.rotatez_path
elif data == 'twist':
self.current_motion = self.twist_path
else:
self.current_motion = None
self.cmd_queue.task_done()
break break
def standby(self):
dest = self.standby_posture
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 laydown(self):
dest = self.laydown_posture
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)
@ -355,42 +298,12 @@ class Hexapod(Thread):
time.sleep(self.interval) time.sleep(self.interval)
pass pass
else: else:
data = cmd_string.split(':')[-2] self.cmd_handler(cmd_string)
if data == 'standby':
self.current_motion = None if self.current_motion['type'] == 'motion':
self.standby() self.motion(self.current_motion['coord'])
elif data == 'forward': elif self.current_motion['type'] == 'posture':
self.current_motion = self.forward_path self.posture(self.current_motion['coord'])
elif data == 'backward':
self.current_motion = self.backward_path
elif data == 'fastforward':
self.current_motion = self.fastforward_path
elif data == 'fastbackward':
self.current_motion = self.fastbackward_path
elif data == 'leftturn':
self.current_motion = self.leftturn_path
elif data == 'rightturn':
self.current_motion = self.rightturn_path
elif data == 'shiftleft':
self.current_motion = self.shiftleft_path
elif data == 'shiftright':
self.current_motion = self.shiftright_path
elif data == 'climb':
self.current_motion = self.climb_path
elif data == 'rotatex':
self.current_motion = self.rotatex_path
elif data == 'rotatey':
self.current_motion = self.rotatey_path
elif data == 'rotatez':
self.current_motion = self.rotatez_path
elif data == 'twist':
self.current_motion = self.twist_path
else:
self.current_motion = None
self.cmd_queue.task_done()
if self.current_motion is not None:
self.move_routine(self.current_motion)
def main(): def main():

Write Preview
Loading…
Cancel
Save