update
This commit is contained in:
parent
1d913a81d7
commit
91d295ff8b
@ -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__':
|
||||
|
92
software/rpi/tcpserver.py
Normal file
92
software/rpi/tcpserver.py
Normal file
@ -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')
|
Loading…
x
Reference in New Issue
Block a user