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

control through TCP

Browse Source
master
Zhengyu Peng 3 years ago
parent 0c11f17a7f
commit f44c60590b
1 changed files with 178 additions and 30 deletions
Show Stats Download Patch File Download Diff File

208
software/rpi/hexapod.py
Unescape Escape View File

@ -42,8 +42,18 @@ from path_generator import gen_climb_path
from path_generator import gen_rotatex_path, gen_rotatey_path, gen_rotatez_path from path_generator import gen_rotatex_path, gen_rotatey_path, gen_rotatez_path
from path_generator import gen_twist_path from path_generator import gen_twist_path
import socket
import errno
class Hexapod: class Hexapod:
ERROR = -1
LISTEN = 1
CONNECTED = 2
STOP = 3
SIG_NORMAL = 0
SIG_STOP = 1
SIG_DISCONNECT = 2
def __init__(self): def __init__(self):
# x -> right # x -> right
@ -57,6 +67,13 @@ class Hexapod:
with open('./config.json', 'r') as read_file: with open('./config.json', 'r') as read_file:
self.config = json.load(read_file) self.config = json.load(read_file)
self.ip = '192.168.1.127'
self.port = 1234
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# self.tcp_socket.settimeout(1)
self.signal = self.SIG_NORMAL
self.mount_x = np.array(self.config['legMountX']) self.mount_x = np.array(self.config['legMountX'])
self.mount_y = np.array(self.config['legMountY']) self.mount_y = np.array(self.config['legMountY'])
self.root_j1 = self.config['legRootToJoint1'] self.root_j1 = self.config['legRootToJoint1']
@ -123,39 +140,41 @@ class Hexapod:
self.rotatez_path = gen_rotatez_path(self.standby_coordinate) self.rotatez_path = gen_rotatez_path(self.standby_coordinate)
self.twist_path = gen_twist_path(self.standby_coordinate) self.twist_path = gen_twist_path(self.standby_coordinate)
self.current_motion = None
self.standby() self.standby()
time.sleep(1) time.sleep(1)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.forward_path, 0.005) # self.move(self.forward_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.backward_path, 0.005) # self.move(self.backward_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.fastforward_path, 0.005) # self.move(self.fastforward_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.fastbackward_path, 0.005) # self.move(self.fastbackward_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.leftturn_path, 0.005) # self.move(self.leftturn_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.rightturn_path, 0.005) # self.move(self.rightturn_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.shiftleft_path, 0.005) # self.move(self.shiftleft_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.shiftright_path, 0.005) # self.move(self.shiftright_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.climb_path, 0.005) # self.move(self.climb_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.rotatex_path, 0.005) # self.move(self.rotatex_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.rotatey_path, 0.005) # self.move(self.rotatey_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.rotatez_path, 0.005) # self.move(self.rotatez_path, 0.005)
for mm in range(0, 10): # for mm in range(0, 10):
self.move(self.twist_path, 0.005) # self.move(self.twist_path, 0.005)
time.sleep(1) # time.sleep(1)
self.standby() # self.standby()
def calculate_standby_coordinate(self, j2_angle, j3_angle): def calculate_standby_coordinate(self, j2_angle, j3_angle):
j2_rad = j2_angle/180*np.pi j2_rad = j2_angle/180*np.pi
@ -187,6 +206,22 @@ class Hexapod:
time.sleep(interval) time.sleep(interval)
def move_routine(self, path, interval):
for p_idx in range(0, np.shape(path)[0]):
dest = path[p_idx, :, :]
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, :])
time.sleep(interval)
def standby(self): def standby(self):
dest = self.standby_coordinate dest = self.standby_coordinate
angles = self.inverse_kinematics(dest) angles = self.inverse_kinematics(dest)
@ -232,10 +267,123 @@ class Hexapod:
return angles return angles
def run(self):
try:
self.tcp_socket.bind((self.ip, self.port))
# self.tcp_socket.setblocking(False)
self.tcp_socket.listen(1)
print('TCP listening')
except OSError as err:
# print('emit tcp server error')
# self.status.emit(self.STOP, '')
pass
else:
# self.status.emit(self.LISTEN, '')
while True:
# Wait for a connection
# print('wait for a connection')
# self.status.emit(self.LISTEN, '')
try:
self.connection, addr = self.tcp_socket.accept()
self.connection.setblocking(False)
# self.connection.settimeout(1)
print('New connection')
except socket.timeout as t_out:
pass
else:
while True:
# print('waiting for data')
# if self.signal == self.SIG_NORMAL:
try:
data = self.connection.recv(4096)
except socket.error as e:
err = e.args[0]
if err == errno.EAGAIN or err == errno.EWOULDBLOCK:
if self.current_motion is not None:
self.move(self.current_motion, 0.005)
else:
time.sleep(1)
print('No data available')
continue
else:
# a "real" error occurred
print(e)
break
# sys.exit(1)
else:
if data:
if data.decode()=='standby':
self.current_motion = None
self.standby()
elif data.decode()=='forward':
self.current_motion = self.forward_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='backward':
self.current_motion = self.backward_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='fastforward':
self.current_motion = self.fastforward_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='fastbackward':
self.current_motion = self.fastbackward_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='leftturn':
self.current_motion = self.leftturn_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='rightturn':
self.current_motion = self.rightturn_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='shiftleft':
self.current_motion = self.shiftleft_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='shiftright':
self.current_motion = self.shiftright_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='climb':
self.current_motion = self.climb_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='rotatex':
self.current_motion = self.rotatex_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='rotatey':
self.current_motion = self.rotatey_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='rotatez':
self.current_motion = self.rotatez_path
# self.move(self.current_motion, 0.005)
elif data.decode()=='twist':
self.current_motion = self.twist_path
# self.move(self.current_motion, 0.005)
else:
print(data.decode())
self.current_motion = None
# move_routine(self, path, interval)
# self.message.emit(
# addr[0]+':'+str(addr[1]),
# data.decode())
else:
# self.status.emit(self.LISTEN, '')
self.current_motion = None
self.standby()
break
# elif self.signal == self.SIG_DISCONNECT:
# self.signal = self.SIG_NORMAL
# # self.connection.close()
# # self.status.emit(self.LISTEN, '')
# break
finally:
self.tcp_socket.close()
self.current_motion = None
self.standby()
# self.status.emit(self.STOP, '')
print('exit')
def main(): def main():
hexapod = Hexapod() hexapod = Hexapod()
hexapod.run()
if __name__ == '__main__': if __name__ == '__main__':

Write Preview
Loading…
Cancel
Save