#!/usr/bin/env python3 import serial, atexit, threading, socket, sys, signal import time import binascii import pymodbus.utilities as modCRC import pymodbus from pymodbus.client.sync import ModbusSerialClient as ModbusClient from pymodbus.exceptions import * from pymodbus.register_read_message import ReadHoldingRegistersResponse from thread_shutdown import * # Registers OUTPUT_REG = 0x03E8 INPUT_REG = 0x07D0 #Presets GRIPPER_RESET = [0x0000, 0x0000, 0x0000, 0x0000] GRIPPER_ACTIVATE = [0x0100, 0x0000, 0x0000, 0x0000] class RQ_2F_RTU(threading.Thread): def __init__(self, slave_id=9, com_port=None, stopper=None): threading.Thread.__init__(self) self._stop_event = stopper self.id = slave_id self.gripper = ModbusClient(method='rtu', port=com_port, stopbits=1, parity='N', baudrate=115200, retries=5, retry_on_empty=True,timeout=0.2) self.is_connected = False self.threads = [threading.Thread(target=self._socket_server), threading.Thread(target=self._stream_status)] self.gripper_status = 0 self.fault_status = 0 self.pos_req_echo = 0 self.position = 0 self.current = 0 self.status_register = { 'OBJ':0, 'STA':0, 'GTO':0, 'ACT':0} self.fault_register = { 'kFLT':0, 'gFLT':0 } self.position_echo = 0 self.position = 0 self.current = 0 # atexit.register(self.exit_handler) # def __del__(self): # self.stop() # self.gripper.close() def exit_handler(self): "Closing Serial port" self.gripper.close() def activate(self, wait=False, reset=False): if not self.is_connected: print("Gripper not connected yet") return if reset: self.reset() rq = self.gripper.write_registers(OUTPUT_REG, [0x0100,0x0000,0x0000], unit=self.id) if isinstance(rq, ModbusIOException): return False if wait: while self.status_register['STA'] != 0x03: print("Waiting to reach target") time.sleep(0.01) print("Target reached") time.sleep(0.5) return True def reset(self): rq = self.gripper.write_registers(OUTPUT_REG, [0x0000,0x0000,0x0000], unit=self.id) # print(rq) def is_activated(self): return bool(self.status_register['ACT']) def get_status_register(self): # rr = self.gripper.read_holding_registers(INPUT_REG, 1, unit=self.id) rr = self.read_holding(INPUT_REG, 1) status_reg = rr.registers[0] >> 8 reg_dict = self.parse_status_reg(status_reg) self.status_register = reg_dict return (status_reg, reg_dict) def get_status(self): rr = self.read_holding(INPUT_REG, 3) status_reg = rr.registers[0] >> 8 fault_reg = rr.registers[1] >> 8 pos_echo = rr.registers[1] & 0xFF position = rr.registers[2] >> 8 current = rr.registers[2] & 0xFF status_dict = self.parse_status_reg(status_reg) fault_dict = self.parse_fault_reg(fault_reg) self.status_register = status_dict self.fault_register = fault_dict self.position_echo = pos_echo self.position = position self.current = current return (status_dict, fault_dict, pos_echo, position, current) def parse_status_reg(self, status_reg): reg_dict = { 'OBJ':status_reg >> 6, 'STA':(status_reg >> 4)&0b00000011, 'GTO':(status_reg >> 3)&1, 'ACT': status_reg & 1 } return reg_dict def parse_fault_reg(self, fault_reg): fault_dict = { 'kFLT':fault_reg>>4, 'gFLT':fault_reg&0x0F } return fault_dict def read_holding(self, address, num): if not self.is_connected: print("Gripper not connected yet") return #To get around spuratic - object has no attribute 'registers' rr = self.gripper.read_holding_registers(address, num, unit=self.id,timeout=2) while not isinstance(rr, ReadHoldingRegistersResponse): rr = self.gripper.read_holding_registers(address, num, unit=self.id) time.sleep(0.01) return rr def set_gripper(self, position, speed, force, wait=False): assert position in range(0, 256) assert speed in range(0, 256) assert force in range(0, 256) if not self.is_connected: print("Gripper not connected yet") return if not self.is_activated(): print("Gripper must be activated") return w0 = 0x0900 w1 = 0x0000 | position w2 = (speed << 8) | force rq = self.gripper.write_registers(OUTPUT_REG, [w0,w1,w2], unit=self.id) time.sleep(0.05) if wait: while self.status_register['OBJ'] == 0x00: # and self.status_register['GTO'] == 1 # print("Waiting to reach target position: %d"%position) # print(self.status_register, self.position, self.position_echo) # print(self.fault_register) time.sleep(0.05) print("Target reached") def close_gripper(self, force=255, wait=False): self.set_gripper(255, 255, force, wait) def open_gripper(self, force=255, wait=False): self.set_gripper(0, 255, force, wait) def align_gripper(self, force=255): self.close_gripper(wait=True) self.open_gripper(wait=True) def _socket_server(self): HOST = '127.0.0.1' # Standard loopback interface address (localhost) PORT = 8080 # Port to listen on (non-privileged ports are > 1023) with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.bind((HOST, PORT)) while(not self._stop_event.is_set()): try: s.settimeout(0.2) s.listen(1) conn, addr = s.accept() conn.settimeout(0.2) except socket.timeout: pass else: with conn: print('Connected by', addr) conn.sendall(b'Connected to gripper server') while not self._stop_event.is_set(): try: data = conn.recv(1024).decode() if 'close' in data: self.close_gripper(wait=True) conn.sendall(b'Closed Gripper\n') elif 'min_c' in data: self.set_gripper(100, 255,255, wait=True) conn.sendall(b'Min closed gripper\n') elif 'min_o' in data: self.set_gripper(85, 255,255, wait=True) conn.sendall(b'Min openened gripper\n') elif 'open' in data: self.open_gripper(wait=False) conn.sendall(b'Opend Gripper\n') elif 'align' in data: self.align_gripper() conn.sendall(b'Aligned Gripper\n') elif 'reset' in data: self.reset() conn.sendall(b'Reset Gripper\n') elif 'activate' in data: self.activate() conn.sendall(b'Gripper Activated\n') elif 'activate_r' in data: self.reset() self.activate(wait=True) conn.sendall(b'Gripper reset and activated\n') else: conn.sendall(b'Command not recognised\n') except socket.timeout: pass except ConnectionAbortedError: break print("Connection with", addr, "disconnected") def connect(self): self.is_connected = self.gripper.connect() try: print("Testing Connection") if not self.activate(): raise Exception except: self.is_connected = False if self.is_connected: print("Gripper Connected") self._stop_event.clear() for t in self.threads: current_target = t._target t = threading.Thread(target=current_target) t.start() return True else: print("Connection Failed") self.gripper.close() return False def close(self): print("Gripper Disconnected") self.stop() time.sleep(0.5) #needs time for threads to finish. Despite calling join() self.gripper.close() self.is_connected = False def _stream_status(self): while not self._stop_event.is_set(): self.get_status() time.sleep(0.01) def stop(self): self._stop_event.set() for t in self.threads: if t.is_alive(): t.join() if __name__ == "__main__": stop_event_flag = threading.Event() signal.signal(signal.SIGTERM, service_shutdown) signal.signal(signal.SIGINT, service_shutdown) try: gripper = RQ_2F_RTU(slave_id=9, com_port='COM9',stopper=stop_event_flag) if not gripper.connect(): raise ServiceExit gripper.reset() # gripper.align_gripper() while(True): time.sleep(0.5) except ServiceExit: stop_event_flag.set() gripper.close() gripper.close() #from RQ_2F_Gripper import RQ_2F_RTU #gripper.align_gripper()