diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py index 01b714b..f3d05c2 100644 --- a/software/rpi/hexapod.py +++ b/software/rpi/hexapod.py @@ -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.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() + # 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) + + # 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__':