-
Notifications
You must be signed in to change notification settings - Fork 0
/
CarvingDriver.py
204 lines (188 loc) · 8.14 KB
/
CarvingDriver.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
"""
Server version: {tcp_gateway,{ok,"4.67.1-r91102","1.0"}}.
Last update: 09 June 2020
Created: 08 June 2020
A class which allows to talk to MCU8 motor control unit (SPECS) via TCP connection.
The reply is typically divided as follows:
0. Read 200 bytes, timeout 2 sec
1. Wait loop with read 1000 bytes, timeout 0.005 sec until timeout error
2. Optional for some commands: Wait loop with read 1000 bytes, timeout 0.01 sec
Timeout and byte size parameters taken from Labview example gateway in SPECS installation folder
@author: Victor Rogalev
"""
import PyQt5.QtCore
import PyQt5.QtWidgets
import time
import socket
import select
import logging
class CarvingControlDriver(PyQt5.QtCore.QThread):
actual_position_signal = PyQt5.QtCore.pyqtSignal('QString')
new_position_signal = PyQt5.QtCore.pyqtSignal('PyQt_PyObject')
actual_status_signal = PyQt5.QtCore.pyqtSignal('QString')
def __init__(self, host, port, **kwargs):
"""Initialize TCP connection and get the control ready
:type host: string
:type port: integer
"""
super(self.__class__, self).__init__()
self.connection_flag = False
self.host = host
self.port = port
self.init_check = False
print('establishing connection with host, port', self.host, self.port)
self.mySocket = socket.socket()
"""Initial socket connection - the reply is expected to be {tcp_gateway,{ok,"4.67.1-r91102","1.0"}}."""
try:
self.mySocket.connect((self.host, self.port))
self.mySocket.setblocking(0)
timeout = 2
ready = select.select([self.mySocket], [], [], timeout)
if ready[0]:
self.send_reply = self.mySocket.recv(200).decode()
print(self.send_reply)
else:
self.send_reply = ""
print("no reply from server during socket connection")
if "{tcp_gateway,{ok,\"4.67.1-r91102\",\"1.0\"}}." in self.send_reply:
self.connection_flag = True
except Exception as e:
logging.exception(e)
self.connection_flag = False
self.mySocket.close()
print('connection failed, socket closed')
"""Sequence of commands to get the manipulator control ready"""
if self.connection_flag:
try:
local_reply = self.send_command("{req,'MCU8',clear_owner}.\r\n")
except Exception as e:
logging.exception(e)
print("{req,'MCU8',clear_owner}. failed")
local_reply = ""
pass
if "ok" in local_reply:
try:
local_reply = self.send_command("{req,'MCU8',force_owner}.\r\n")
except Exception as e:
logging.exception(e)
print("{req,'MCU8',force_owner}. failed")
local_reply = ""
pass
if "ok" in local_reply:
try:
local_reply = self.send_command("{req,'MCU8',go_online}.\r\n")
except Exception as e:
logging.exception(e)
print("{req,'MCU8',go_online}. failed")
local_reply = ""
pass
if "ok" in local_reply:
try:
local_reply = self.send_command("{req,'MCU8',get_state}.\r\n")
self.init_check = True
except Exception as e:
logging.exception(e)
print("{req,'MCU8',get_state}. failed")
local_reply = ""
pass
# self.init_check = True # TEST MODE, REMOVE LATER!!!
def start(self):
"""Every self.timing [ms] checking connection with server and trying to get positions"""
if self.init_check:
self.timing = 1000
self.timer_x = PyQt5.QtCore.QTimer(self)
self.timer_x.timeout.connect(self.get_position)
self.timer_x.start(self.timing)
"Start monitoring the state of the carving"
time.sleep(0.2)
self.start_busy_monitor()
else:
print("error while initial check of the manipulator")
def start_busy_monitor(self):
"Check busy/idle status of the manipulator"
if self.init_check:
self.timing2 = 500
self.timer_busy_monitor = PyQt5.QtCore.QTimer(self)
self.timer_busy_monitor.timeout.connect(self.get_state)
self.timer_busy_monitor.start(self.timing2)
else:
print("error while initial check of the manipulator")
def stop_manipulator(self):
self.timer_x.stop()
self.timer_busy_monitor.stop()
self.send_command("{req, 'MCU8', stop}.\r\n")
self.timer_x.start(self.timing)
self.self.timer_busy_monitor.start(self.timing2)
def get_position(self):
""" Get axis positions from MCU8 """
actual_positions = self.send_command("{req,'MCU8',get_position}.\r\n")
# actual_positions = "{'MCU8',{ok,[{axis_pos,0,8.000244140625},{axis_pos,1,-7.999755859375},{axis_pos,2,202.00006103515625},{axis_pos,3,-130.00079013677276},{axis_pos,4,0.0},{axis_pos,5,0.0}]}}"
self.actual_position_signal.emit(actual_positions)
def get_state(self):
"""Get manipulator state: idle or busy"""
carving_state = self.send_command("{req,'MCU8',get_state}.\r\n")
# carving_state = "{'MCU8',{ok,idle}}"
self.actual_status_signal.emit(carving_state)
def set_position(self, new_position):
"""
:type new_position: list
Position must be a list of 6 values (float,None) separated by , . NONE value means no command to move this axis.
"""
self.timer_x.stop()
self.timer_busy_monitor.stop()
self.new_position = new_position
"""emit predefined positions to update target lineedits"""
if None not in self.new_position:
self.new_position_signal.emit(self.new_position)
new_command = "{req,'MCU8',{set_position,["
for i in range(len(self.new_position)):
if self.new_position[i] is not None:
new_command += '{axis_pos,' + f'{i},' + '{:.5f}'.format(self.new_position[i]) + '},'
final_command = new_command[:-1] + "]}}.\r\n"
self.send_command(final_command)
self.timer_x.start(self.timing)
self.timer_busy_monitor.start(self.timing2)
def send_command(self, command):
""" Ask server and receive reply """
try:
self.message = command
# print('attempt to send: ', self.message)
self.mySocket.setblocking(0)
self.mySocket.send(self.message.encode())
# print('message send')
timeout = 2
ready = select.select([self.mySocket], [], [], timeout)
if ready[0]:
self.send_reply = self.mySocket.recv(200).decode()
else:
# print("no send reply from server")
self.send_reply = ""
loop_flag = True
while loop_flag:
timeout = 0.005
ready = select.select([self.mySocket], [], [], timeout)
if ready[0]:
self.wait_reply = self.mySocket.recv(1000).decode()
else:
# print("no wait reply from server")
self.wait_reply = ""
loop_flag = False
self.reply = self.send_reply + self.wait_reply
# print(self.reply)
except Exception as e:
logging.exception(e)
self.reply = ""
print('error sending/receiving a command from manipulator server')
self.connection_flag = False
pass
return self.reply
def close(self):
self.stop_manipulator()
try:
self.mySocket.close()
except:
pass
self.timer_x.stop()
self.timer_x.deleteLater()
self.timer_busy_monitor.stop()
self.timer_busy_monitor.deleteLater()