control through TCP
This commit is contained in:
Zhengyu Peng
parent
0c11f17a7f
commit
f44c60590b
@ -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_twist_path
|
||||
|
||||
import socket
|
||||
import errno
|
||||
|
||||
class Hexapod:
|
||||
ERROR = -1
|
||||
LISTEN = 1
|
||||
CONNECTED = 2
|
||||
STOP = 3
|
||||
|
||||
SIG_NORMAL = 0
|
||||
SIG_STOP = 1
|
||||
SIG_DISCONNECT = 2
|
||||
|
||||
def __init__(self):
|
||||
# x -> right
|
||||
@ -57,6 +67,13 @@ class Hexapod:
|
||||
with open('./config.json', 'r') as 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_y = np.array(self.config['legMountY'])
|
||||
self.root_j1 = self.config['legRootToJoint1']
|
||||
@ -123,39 +140,41 @@ class Hexapod:
|
||||
self.rotatez_path = gen_rotatez_path(self.standby_coordinate)
|
||||
self.twist_path = gen_twist_path(self.standby_coordinate)
|
||||
|
||||
self.current_motion = None
|
||||
|
||||
self.standby()
|
||||
time.sleep(1)
|
||||
|
||||
for mm in range(0, 10):
|
||||
self.move(self.forward_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.backward_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.fastforward_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.fastbackward_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.leftturn_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.rightturn_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.shiftleft_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.shiftright_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.climb_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.forward_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.backward_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.fastforward_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.fastbackward_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.leftturn_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.rightturn_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.shiftleft_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.shiftright_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.climb_path, 0.005)
|
||||
|
||||
for mm in range(0, 10):
|
||||
self.move(self.rotatex_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.rotatey_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.rotatez_path, 0.005)
|
||||
for mm in range(0, 10):
|
||||
self.move(self.twist_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.rotatex_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.rotatey_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.rotatez_path, 0.005)
|
||||
# for mm in range(0, 10):
|
||||
# self.move(self.twist_path, 0.005)
|
||||
|
||||
time.sleep(1)
|
||||
self.standby()
|
||||
# time.sleep(1)
|
||||
# self.standby()
|
||||
|
||||
def calculate_standby_coordinate(self, j2_angle, j3_angle):
|
||||
j2_rad = j2_angle/180*np.pi
|
||||
@ -186,6 +205,22 @@ class Hexapod:
|
||||
self.leg_3.move_junctions(angles[3, :])
|
||||
|
||||
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):
|
||||
dest = self.standby_coordinate
|
||||
@ -232,10 +267,123 @@ class Hexapod:
|
||||
|
||||
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():
|
||||
|
||||
hexapod = Hexapod()
|
||||
hexapod.run()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user