diff --git a/UPICCO_new.py b/UPICCO_new.py new file mode 100644 index 0000000000000000000000000000000000000000..878f93cfc0d2b01fc4c201da78a2aac896a5613c --- /dev/null +++ b/UPICCO_new.py @@ -0,0 +1,122 @@ +import clr +import time + +clr.AddReference("UsbDllWrap") + +from Newport.USBComm import * +from System.Text import StringBuilder +from System.Collections import Hashtable +from System.Collections import IDictionaryEnumerator + +class UPICCO: + def __init__(self, name='align', dev='8742 97165', sn='NA', num_motors=2): + self.name = name + self.dev = dev + self.sn = sn + self.num_motors = num_motors + self.positions = [None] * self.num_motors + self.velocities = [None] * self.num_motors + self.accelerations = [None] * self.num_motors + self.usb = USB(True) + self.status = self.usb.OpenDevices(0, True) + + for i in range(self.num_motors): + try: + response = StringBuilder(64) + self.usb.Query(self.dev, f'{i+1}TP?', response) + self.positions[i] = response.ToString() + + response = StringBuilder(64) + self.usb.Query(self.dev, f'{i+1}VA?', response) + self.velocities[i] = response.ToString() + + response = StringBuilder(64) + self.usb.Query(self.dev, f'{i+1}AC?', response) + self.accelerations[i] = response.ToString() + except: + print(f'Failed to get motor {i+1} data') + continue + + def get_motor_position(self, motor): + try: + response = StringBuilder(64) + self.usb.Query(self.dev, f'{motor}TP?', response) + self.positions[motor-1] = response.ToString() + return f'[{self.name}: DONE MOTOR POSITION={self.positions[motor-1]}]' + except: + return 'UPICCO: ERROR TO GET MOTOR POSITION' + + def get_motor_velocity(self, motor): + try: + response = StringBuilder(64) + self.usb.Query(self.dev, f'{motor}VA?', response) + self.velocities[motor-1] = response.ToString() + return f'{self.name}: DONE MOTOR VELOCITY={self.velocities[motor-1]}' + except: + return 'UPICCO: ERROR TO GET MOTOR VELOCITY' + + def get_motor_acceleration(self, motor): + try: + response = StringBuilder(64) + self.usb.Query(self.dev, f'{motor}AC?', response) + self.accelerations[motor-1] = response.ToString() + return f'{self.name}: DONE MOTOR ACCELERATION={self.accelerations[motor-1]}' + except: + return 'UPICCO: ERROR TO GET MOTOR ACCELERATION' + + def set_velocity(self, motor, velocity): + try: + self.usb.Query(self.dev, f'{motor}VA{velocity}', StringBuilder(64)) + self.velocities[motor-1] = velocity + return f'{self.name}: DONE MOTOR VELOCITY={velocity}' + except: + return 'ERROR: Failed to talk with UPICCO' + + def set_acceleration(self, motor=1, acceleration=100000): + try: + self.usb.Query(self.dev, f'{motor}AC{acceleration}', StringBuilder(64)) + self.accelerations[motor-1] = acceleration + return f'{self.name}: DONE MOTOR ACCELERATION={acceleration}' + except: + return 'ERROR: Failed to talk with UPICCO' + + def fget_motor_motion_status(self, motor= 1): + try: + response = StringBuilder(64) + self.usb.Query(self.dev, f'{motor}MD?', response) + return response.ToString() + except: + return 'UPICCO: ERROR TO GET MOTOR STATUS' + + def move_absolute(self, motor=1, steps=0): + try: + self.usb.Query(self.dev, f'{motor}PA{steps}', StringBuilder(64)) + while True: + time.sleep(0.1) + msg = self.fget_motor_motion_status(motor) + if msg == "1": + time.sleep(0.1) + msg = self.get_motor_position(motor) + break + return msg + except: + return 'ERROR: Failed to talk with UPICCO' + + def move_relative(self, motor=1, steps=0): + try: + self.usb.Query(self.dev, f'{motor}PR{steps}', StringBuilder(64)) + while True: + time.sleep(0.1) + msg = self.fget_motor_motion_status(motor) + if msg == "1": + time.sleep(0.1) + msg = self.get_motor_position(motor) + break + return msg + except: + return 'ERROR: Failed to talk with UPICCO' + + def close(self): + if self.status: + self.usb.CloseDevices() + print("Devices Closed.\n")