From 91d295ff8bb3f42ae157218ea9da1b74735c5c3d Mon Sep 17 00:00:00 2001
From: Zhengyu Peng <rookiepeng@users.noreply.github.com>
Date: Wed, 29 Dec 2021 20:34:14 -0500
Subject: [PATCH] update

---
 software/rpi/hexapod.py   | 26 +++++++++--
 software/rpi/tcpserver.py | 92 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 114 insertions(+), 4 deletions(-)
 create mode 100644 software/rpi/tcpserver.py

diff --git a/software/rpi/hexapod.py b/software/rpi/hexapod.py
index 4ba2242..43f7a2f 100644
--- a/software/rpi/hexapod.py
+++ b/software/rpi/hexapod.py
@@ -30,6 +30,8 @@ from adafruit_servokit import ServoKit
 
 from leg import Leg
 
+from queue import Queue
+
 # python3-numpy
 import numpy as np
 import time
@@ -44,9 +46,12 @@ from path_generator import gen_twist_path
 
 import socket
 import errno
+from threading import Thread
+
+from tcpserver import TCPServer
 
 
-class Hexapod:
+class Hexapod(Thread):
     ERROR = -1
     LISTEN = 1
     CONNECTED = 2
@@ -56,7 +61,8 @@ class Hexapod:
     SIG_STOP = 1
     SIG_DISCONNECT = 2
 
-    def __init__(self):
+    def __init__(self, in_cmd_queue):
+        self.cmd_queue = in_cmd_queue
         # x -> right
         # y -> front
         # z -> up
@@ -278,6 +284,10 @@ class Hexapod:
         return angles
 
     def run(self):
+        self.input_queue.get()
+
+
+        self.input_queue.task_done()
         try:
             self.tcp_socket.bind((self.ip, self.port))
             # self.tcp_socket.setblocking(False)
@@ -392,8 +402,16 @@ class Hexapod:
 
 def main():
 
-    hexapod = Hexapod()
-    hexapod.run()
+    q = Queue()
+    # hexapod = Hexapod()
+    # hexapod.run()
+    tcp_server = TCPServer(q)
+    tcp_server.start()
+    # t1 = Motion(config['motion'], q)
+
+    # t.start()
+    # t1.start()
+    # t2.start()
 
 
 if __name__ == '__main__':
diff --git a/software/rpi/tcpserver.py b/software/rpi/tcpserver.py
new file mode 100644
index 0000000..9f8ec34
--- /dev/null
+++ b/software/rpi/tcpserver.py
@@ -0,0 +1,92 @@
+import socket
+import errno
+from threading import Thread
+import json
+
+
+class TCPServer(Thread):
+    ERROR = -1
+    LISTEN = 1
+    CONNECTED = 2
+    STOP = 3
+
+    SIG_NORMAL = 0
+    SIG_STOP = 1
+    SIG_DISCONNECT = 2
+
+    def __init__(self, out_cmd_queue):
+        self.cmd_queue = out_cmd_queue
+
+        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.signal = self.SIG_NORMAL
+
+    def run(self):
+        try:
+            self.tcp_socket.bind((self.ip, self.port))
+            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:
+                                self.cmd_queue.put(data.decode())
+                                # move_routine(self, path, interval)
+                                # self.message.emit(
+                                #     addr[0]+':'+str(addr[1]),
+                                #     data.decode())
+                            else:
+                                # self.status.emit(self.LISTEN, '')
+                                self.cmd_queue.put('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.cmd_queue.put('standby')
+            # self.status.emit(self.STOP, '')
+            print('exit')