def __init__(self, receive_callback=None, debug=False):
self.debug = debug
self.serial_device = SerialDevice()
self.receive_callback = receive_callback
self.get_status_callback = None
# status
self.status = {
'state': None,
'angle': {
'a1': 0.0,
'a2': 0.0,
'a3': 0.0,
'a4': 0.0,
'a5': 0.0,
'a6': 0.0,
'rail': 0.0,
},
'cartesian': {
'tx': 0.0,
'ty': 0.0,
'tz': 0.0,
'rx': 0.0,
'ry': 0.0,
'rz': 0.0,
},
'pump_pwm': 0,
'valve_pwm': 0,
'motion_mode': 0,
}
def connectClicked_Callback(self):
if self.dev == None:
self.dev = SerialDevice(self.port)
self.setWidgetEnableOnConnect()
else:
self.connectPushButton.setText('Connect')
self.dev.close()
self.dev = None
self.setWidgetEnableOnDisconnect()
def ble(self):
self.device = BleDevice(self)
logging.info('Starting Sparklet')
self.device.start()
logging.info('Hit <ENTER> to disconnect')
input()
self.device.stop()
self.stop_animation_process()
logging.info('Sparklet stopped')
class MasterArmV2:
# Device IDs to allow for the axis config.
deviceIDs = [0x01, 0x02, 0x03, 0x04, 0x05]
# create an instance of SerialDevice()
serial_dev = SerialDevice()
# that instance is passed as argument to BplSerial
global_serial = BplSerial(serial_dev, len(deviceIDs))
def __init__(self):
self.deviceIDs = [0xC1, 0xC2, 0xC3, 0xC4, 0xC5]
self.global_serial.deviceIDs = self.deviceIDs
def usb_list(self):
'''
Prints a list of available serial ports.
This is a helper function and is non-essential.
:return: None
'''
comports = serial.tools.list_ports.comports()
print('***\nCOM PORT LIST:')
for comport in comports:
print(comport, comport.device)
print('***\n')
def connect(self):
self.global_serial.connect(COM_PORT, BAUD)
def get_packets(self):
packets = self.global_serial.serial_device.readdata()
return packets
def parse_packets(self, packets):
return [
self.global_serial.serial_device.parsepacket(p) for p in packets
]
class SimpleGuiMainWindow(QtGui.QMainWindow,Ui_MainWindow):
def __init__(self,parent=None):
super(SimpleGuiMainWindow,self).__init__(parent)
self.setupUi(self)
self.setupTimer()
self.connectActions()
self.initialize()
def setupTimer(self):
"""
Setup timer object
"""
self.timer = QtCore.QTimer()
self.timer.setInterval(TIMER_INTERVAL_MS)
try:
self.timer.timeout.connect(self.timer_Callback)
except AttributeError:
self.connect(self.timer,QtCore.SIGNAL("timeout()"),self.timer_Callback)
def connectActions(self):
self.portLineEdit.editingFinished.connect(self.portChanged_Callback)
self.connectPushButton.pressed.connect(self.connectPressed_Callback)
self.connectPushButton.clicked.connect(self.connectClicked_Callback)
self.measurePushButton.clicked.connect(self.measureClicked_Callback)
def initialize(self):
self.port = DFLT_PORT
self.portLineEdit.setText(self.port)
self.setWidgetEnableOnDisconnect()
self.dev = None
self.isLogging = False
self.logPath = '/Users/x1sc0/datalog.txt';
self.createLogFile()
def portChanged_Callback(self):
self.port = str(self.portLineEdit.text())
def connectPressed_Callback(self):
if self.dev == None:
self.connectPushButton.setText('Disconnect')
self.portLineEdit.setEnabled(False)
def connectClicked_Callback(self):
if self.dev == None:
self.dev = SerialDevice(self.port)
self.setWidgetEnableOnConnect()
else:
self.connectPushButton.setText('Connect')
self.dev.close()
self.dev = None
self.setWidgetEnableOnDisconnect()
def measureClicked_Callback(self):
if self.isLogging:
self.timer.stop()
self.isLogging = False
self.pwrTextEdit.setText("")
self.ldrTextEdit.setText("")
else:
self.timer.start()
self.isLogging = True
def timer_Callback(self):
data = self.dev.getMeasurement()
pwr = str(data[0])
ldr = str(data[1])
self.pwrTextEdit.setText(pwr)
self.ldrTextEdit.setText(ldr)
self.logger.log(logging.DEBUG,data)
def createLogFile(self):
logging.basicConfig(filename=self.logPath,
level=logging.DEBUG,
format='%(asctime)s %(message)s',
datefmt='%m/%d/%Y %I:%M:%S')
self.logger = logging.getLogger(__name__)
def setWidgetEnableOnConnect(self):
self.pwrTextEdit.setEnabled(True)
self.ldrTextEdit.setEnabled(True)
self.measurePushButton.setEnabled(True)
self.portLineEdit.setEnabled(False)
def setWidgetEnableOnDisconnect(self):
self.pwrTextEdit.setEnabled(False)
self.ldrTextEdit.setEnabled(False)
self.measurePushButton.setEnabled(False)
self.portLineEdit.setEnabled(True)
def main(self):
self.show()
class Reach5Mini():
"""Reach5Mini Class: Example class used for interfacing with the Reach5Mini attached on the serial port (COM_PORT)
Has example functionality for sending commands (eg. position, velocity, mode),
and requesting data (eg. position, current, mode, serial number).
"""
# 5 device IDs to allow for the 5 axis config.
deviceIDs = [0x01, 0x02, 0x03, 0x04, 0x05]
# create an instance of SerialDevice()
serial_dev = SerialDevice()
# that instance is passed as argument to BplSerial
global_serial = BplSerial(serial_dev, len(deviceIDs))
# If more than one serial device is required (i.e. for multiple grabbers),
# create additional instances of SerialDevice() and BplSerial()
# EG: serial_dev_2 = SerialDevice()
# global_serial_2 = BplSerial(serial_dev_2)
global_serial.deviceIDs = deviceIDs
def connect(self):
self.global_serial.connect(COM_PORT, BAUD)
def usb_list(self):
'''
Prints a list of available serial ports.
This is a helper function and is non-essential.
:return: None
'''
comports = serial.tools.list_ports.comports()
print('***\nCOM PORT LIST:')
for comport in comports:
print(comport, comport.device)
print('***\n')
def enable_all(self):
'''
Axes must be enabled before they will respond to further commands.
:return: None
'''
for devid in self.deviceIDs:
self.global_serial.send_mode(devid, Mode.STANDBY)
print('Reach5Mini enable_all()')
def disable_all(self):
'''
Disable all axes
:return: None
'''
for devid in self.deviceIDs:
self.global_serial.send_mode(devid, Mode.DISABLE)
print('Reach5Mini disable_all()')
def set_position(self, device_id, position):
"""Set device position: in radians for rotate, or millimetre for grabber"""
self.global_serial.send_position(device_id, position)
print('Reach5Mini sent position for device', device_id, ':', position)
def set_velocity(self, device_id, velocity):
"""Set device velocity: in radians per second for rotate, or millimetres per second for grabber"""
self.global_serial.send_velocity(device_id, velocity)
print('Reach5Mini sent velocity for device', device_id, ':', velocity)
def set_openloop(self, device_id, openloop):
"""Set device openloop: in radians per second for rotate, or millimetres per second for grabber -
THIS CAN BE DANGEROUS, NOT RECOMMENDED
"""
self.global_serial.send_openloop(device_id, openloop)
print('Reach5Mini sent openloop for device', device_id, ':', openloop)
def set_current(self, device_id, current):
"""Set device current: in mA - THIS CAN BE DANGEROUS, NOT RECOMMENDED"""
self.global_serial.send_current(device_id, current)
print('Reach5Mini sent current for device', device_id, ':', current)
def set_mode(self, device_id, mode):
"""Set device mode: refer to MODE list in RS1_hardware.py for details"""
self.global_serial.send_mode(device_id, mode)
print('Reach5Mini sent mode for device', device_id, ':', mode)
def get_voltage(self):
'''
See NOTES ON RECEIVING INFO FROM RS1 at top of document
for optimisation of this and subsequent get_... functions
:return: float
'''
self.global_serial.send_request_packet(self.deviceIDs[4],
PacketID.SUPPLYVOLTAGE)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(
self.deviceIDs[4])
voltage = this_motor[PacketID.SUPPLYVOLTAGE]
print('voltage:', str(voltage))
return voltage
def get_mode(self, deviceID):
self.global_serial.send_request_packet(deviceID, PacketID.MODE)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(deviceID)
mode = this_motor[PacketID.MODE]
print('mode:', str(mode))
return mode
def get_current(self, deviceID):
self.global_serial.send_request_packet(deviceID, PacketID.CURRENT)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(deviceID)
current = this_motor[PacketID.CURRENT]
print('current:', str(current))
return current
def get_velocity(self, deviceID):
self.global_serial.send_request_packet(deviceID, PacketID.VELOCITY)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(deviceID)
velocity = this_motor[PacketID.VELOCITY]
print('velocity:', str(velocity))
return velocity
def get_position(self, deviceID):
self.global_serial.send_request_packet(deviceID, PacketID.POSITION)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(deviceID)
position = this_motor[PacketID.POSITION]
print('position:', str(position))
return position
def get_model_number(self):
self.global_serial.send_request_packet(self.deviceIDs[4],
PacketID.MODEL_NUMBER)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(
self.deviceIDs[4])
model_number = this_motor[PacketID.MODEL_NUMBER]
print('model number:', str(model_number))
return model_number
def get_serial_number(self):
self.global_serial.send_request_packet(self.deviceIDs[4],
PacketID.SERIAL_NUMBER)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(
self.deviceIDs[4])
serial_number = this_motor[PacketID.SERIAL_NUMBER]
print('serial number:', str(serial_number))
return serial_number
def get_firmware_version(self):
self.global_serial.send_request_packet(self.deviceIDs[4],
PacketID.VERSION)
time.sleep(
0.1) # give the serial protocol 0.1 seconds to receive the message
self.global_serial.updateMotor(1)
this_motor = self.global_serial.get_motor_by_device_id(
self.deviceIDs[4])
version = this_motor[PacketID.VERSION]
print('firmware version:', str(version))
return version
def __init__(self, receive_callback=None, debug=False):
# The component to which this extension is attached
self.serial_device = SerialDevice()
self.receive_callback = receive_callback
self.debug = debug
class Mirobot:
def __init__(self, receive_callback=None, debug=False):
# The component to which this extension is attached
self.serial_device = SerialDevice()
self.receive_callback = receive_callback
self.debug = debug
#COMMUNICATION #
# send a message
def send_msg(self, msg):
if self.is_connected():
self.serial_device.send(msg, terminator='\r\n')
if self.debug:
print('Message sent: ', msg)
# message receive handler
def _receive_msg(self, msg):
msg = msg.decode('utf-8')
if self.debug:
print('Message received:', msg)
if self.receive_callback is not None:
try:
self.receive_callback(msg)
except Exception as e:
print(e)
print('Receive callback error: ', sys.exc_info()[0])
# check if we are connected
def is_connected(self):
return self.serial_device.is_open
# connect to the mirobot
def connect(self, portname, receive_callback=None):
self.serial_device.portname = portname
self.serial_device.baudrate = 115200
self.serial_device.stopbits = 1
self.serial_device.listen_callback = self._receive_msg
if receive_callback is not None:
self.receive_callback = receive_callback
self.serial_device.open()
# set the receive callback
def set_receive_callback(self, receive_callback):
self.receive_callback = receive_callback
# disconnect from the mirobot
def disconnect(self):
self.serial_device.close()
# COMMANDS #
# home each axis individually
def home_individual(self):
msg = '$HH'
self.send_msg(msg)
# home all axes simultaneously
def home_simultaneous(self):
msg = '$H'
self.send_msg(msg)
# set the hard limit state
def set_hard_limit(self, state):
msg = '$21=' + str(int(state))
self.send_msg(msg)
# set the soft limit state
def set_soft_limit(self, state):
msg = '$21=' + str(int(state))
self.send_msg(msg)
# unlock the shaft
def unlock_shaft(self):
msg = 'M50'
self.send_msg(msg)
# send all axes to their respective zero positions
def go_to_zero(self):
self.go_to_axis(0, 0, 0, 0, 0, 0, 2000)
# send all axes to a specific position
def go_to_axis(self, a1, a2, a3, a4, a5, a6, speed):
msg = 'M21 G90'
msg += ' X' + str(a1)
msg += ' Y' + str(a2)
msg += ' Z' + str(a3)
msg += ' A' + str(a4)
msg += ' B' + str(a5)
msg += ' C' + str(a6)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# increment all axes a specified amount
def increment_axis(self, a1, a2, a3, a4, a5, a6, speed):
msg = 'M21 G91'
msg += ' X' + str(a1)
msg += ' Y' + str(a2)
msg += ' Z' + str(a3)
msg += ' A' + str(a4)
msg += ' B' + str(a5)
msg += ' C' + str(a6)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# point to point move to a cartesian position
def go_to_cartesian_ptp(self, x, y, z, a, b, c, speed):
msg = 'M20 G90 G0'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# linear move to a cartesian position
def go_to_cartesian_lin(self, x, y, z, a, b, c, speed):
msg = 'M20 G90 G1'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# point to point increment in cartesian space
def increment_cartesian_ptp(self, x, y, z, a, b, c, speed):
msg = 'M20 G91 G0'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# linear increment in cartesian space
def increment_cartesian_lin(self, x, y, z, a, b, c, speed):
msg = 'M20 G91 G1'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# set the pwm of the air pump
def set_air_pump(self, pwm):
msg = 'M3S' + str(pwm)
self.send_msg(msg)
# set the pwm of the gripper
def set_gripper(self, pwm):
msg = 'M4E' + str(pwm)
self.send_msg(msg)
def findAnimaticsDevicePort(baudrate):
serialPortList = findSerialDevicePorts()
matchingList = []
for port in serialPortList:
try:
dev = SerialDevice(port=port,
baudrate=baudrate,
timeout=0.1,
write_write_delay=0.1,
write_read_delay=0.1,
debug=DEBUG)
dev.flushInput()
dev.flushOutput()
dev.writeCheckFreq(chr(MOTOR_ADDRESS_OFFSET)+'WAKE ',delay_write=True)
time.sleep(0.1)
dev.writeCheckFreq(chr(MOTOR_ADDRESS_OFFSET)+'WAKE ',delay_write=True)
time.sleep(0.1)
dev.flushInput()
dev.writeCheckFreq(chr(MOTOR_ADDRESS_OFFSET)+'ECHO ',delay_write=True)
dev.readlines()
response = dev.writeRead(chr(MOTOR_ADDRESS_OFFSET)+'RPI ',use_readline=False,check_write_freq=True)
dev.close()
# Animatics response will either be 'RPI 3.141592741 '
# or '3.141592741 ' depending on ECHO setting
# Must enable ECHO so it will work when multiple motors are chained
response = response.replace(chr(MOTOR_ADDRESS_OFFSET)+'RPI ','')
if (0 < len(response)) and (response[0] == '3'):
print("Found an Animatics device!")
return port
except serial.SerialException:
pass
if len(matchingList) == 0:
err_str = 'Could not find Animatics device. Check connections and power or specify port.\n'
err_str += 'Tried ports: ' + str(serialPortList)
raise RuntimeError(err_str)
elif 1 < len(matchingList):
err_str = 'Found more than one Animatics device. Specify port.'
err_str += 'Matching ports: ' + str(matchingList)
raise RuntimeError(err_str)
else:
return matchingList[0]
def serial(self):
self.device = SerialDevice(self)
logging.info('Starting Sparklet')
self.device.start()
self.stop_animation_process()
logging.info('Sparklet stopped')
class Sparklet:
def __init__(self):
signal(SIGINT, self.on_sigint)
self.animations_dir = os.path.join(os.getcwd(), 'animations')
os.makedirs(self.animations_dir, exist_ok=True)
self.protocol = EPXProtocol()
self.animator_process = None
self.command_handlers = [
self.on_connect_headerrq,
self.on_clear_display,
self.on_display_brightness,
self.on_enumerate_animations,
self.on_preview_color,
self.on_upload_frame8,
self.on_upload_animation8,
self.on_upload_pixel8,
self.on_remove_animation,
self.on_store_animation,
self.on_play_stored_animation8,
self.on_play_stored_animation8_byname,
self.on_set_device_name,
self.on_request_thumbnail,
self.on_set_key,
self.on_console_command
]
with open('settings.yaml', 'r') as fh:
self.settings = yaml.load(fh, Loader=yaml.FullLoader)
#self.start_animation_process()
def start_animation_process(self):
#self.animator = Animator()
self.frames_queue = Queue()
self.count = 0
args = (self.frames_queue, )
self.animator_process = Process(target=play_frames, args=args)
self.animator_process.start()
def stop_animation_process(self):
if not self.animator_process:
return
def run_animation_process(self, target, args):
if self.animator_process:
self.animator_process.terminate()
self.animator_process.join()
self.animator_process = Process(target=target, args=args)
self.animator_process.start()
def ble(self):
self.device = BleDevice(self)
logging.info('Starting Sparklet')
self.device.start()
logging.info('Hit <ENTER> to disconnect')
input()
self.device.stop()
self.stop_animation_process()
logging.info('Sparklet stopped')
def serial(self):
self.device = SerialDevice(self)
logging.info('Starting Sparklet')
self.device.start()
self.stop_animation_process()
logging.info('Sparklet stopped')
def on_sigint(self, signum, frame):
self.device.stop()
logging.info('Sparklet stopped')
exit(0)
def on_read_notify(self):
logging.info(f'Sparklet.on_read_notify')
def on_read_request(self, offset):
logging.info(f'Sparklet.on_read_request {offset}')
def on_write_request(self, data, offset):
logging.debug(f'Sparklet.on_write_request received {len(data)} bytes at {datetime.now().strftime("%m/%d/%Y-%I:%M:%S.%f")}')
commands, send_ack = self.protocol.decode(data)
for command in commands:
header, payload = command
logging.info(f'Sparklet.on_write_request {EPXCommand(header.command)} - {header.transaction_id}')
logging.debug(f'Sparklet.on_write_request {payload}')
handler = self.command_handlers[header.command - 1]
success = handler(header, payload)
if send_ack:
frames = self.protocol.encode_binaryack()
logging.debug(f'Sparklet.on_write_request sending BINARY ACK at {datetime.now().strftime("%m/%d/%Y-%I:%M:%S.%f")}')
self._send_frames(frames)
return True
def _send_frames(self, frames):
for frame in frames:
logging.debug(f'Sparklet.on_enumerate_animations sent {len(frame)} bytes')
self.device.write(frame)
return True
def on_connect_headerrq(self, command, payload):
data = {
'status': 'success',
'TransactionID': command.transaction_id,
'data': {
'FIRMWARE': '0.1',
'DEVICENAME': self.device.name,
'CAPABILITIES': ['STORAGE'], #'PREVIEW',
'BATTERY': { 'PCT': 100 },
'DISPLAYWIDTH': DISPLAYWIDTH,
'DISPLAYHEIGHT': DISPLAYHEIGHT,
'AUTOPLAY': 1,
'BRIGHTNESS': self.settings['brightness']
},
}
frames = self.protocol.encode(data, ProtocolFormat.JSON.value, 0)
return self._send_frames(frames)
def on_clear_display(self, header, payload):
self.run_animation_process(clear_display, ())
return True
def on_display_brightness(self, header, payload):
self.settings['brightness'] = payload.Brightness
with open('settings.yaml', 'w') as fh:
yaml.dump(self.settings, fh)
return True
def get_animations_list(self):
files = [f for f in os.listdir(self.animations_dir)]
animations = []
for file in files:
filepath = os.path.join(self.animations_dir, file)
st = os.stat(filepath)
animation = Animation.load(filepath)
item = {}
item['ID'] = animation.id
item['Filename'] = animation.name
item['Size'] = st.st_size
item['UTC'] = animation.utc
animations.append(item)
return animations
def on_enumerate_animations(self, header, payload):
df = os.statvfs('/')
space_used = df.f_bfree / df.f_blocks * 100
data = {
'status': 'success',
'TransactionID': payload.TransactionID,
'StorageUsed': space_used,
'Sequences': self.get_animations_list()
}
frames = self.protocol.encode(data, ProtocolFormat.JSON.value, 0)
return self._send_frames(frames)
def on_preview_color(self, header, payload):
pass
def on_upload_frame8(self, header, payload):
# TODO:
# upload_frame requires a serious amount of CPU
# to support live preview. The support for interprocess
# communication is a bit weak on Python and shared memory
# support is not coming till Python3.8 which is not yet
# supported on Raspberry Pi
# self.frames_queue.put(payload.data)
# self.count += 1
# logging.info(f'put={self.frames_queue.qsize()}')
return True
def on_upload_animation8(self, header, payload):
pass
def on_upload_pixel8(self, header, payload):
pass
def on_remove_animation(self, header, payload):
files = [f for f in os.listdir(self.animations_dir)]
for file in files:
filepath = os.path.join(self.animations_dir, file)
animation = Animation.load(filepath)
if animation.id == payload.ID:
os.remove(filepath)
break
return True
def on_store_animation(self, header, payload):
filename = os.path.join(self.animations_dir, payload.name)
Animation.save(filename, payload)
return True
def on_play_stored_animation8(self, header, payload):
self.run_animation_process(play_by_id, (payload.ID,))
return True
def on_play_stored_animation8_byname(self, header, payload):
self.run_animation_process(play_by_id, (payload.Name,))
return True
def on_set_device_name(self, header, payload):
pass
def on_request_thumbnail(self, header, payload):
pass
def on_set_key(self, header, payload):
pass
def on_console_command(self, header, payload):
pass
class Mirobot:
def __init__(self, receive_callback=None, debug=False):
self.debug = debug
self.serial_device = SerialDevice()
self.receive_callback = receive_callback
self.get_status_callback = None
# status
self.status = {
'state': None,
'angle': {
'a1': 0.0,
'a2': 0.0,
'a3': 0.0,
'a4': 0.0,
'a5': 0.0,
'a6': 0.0,
'rail': 0.0,
},
'cartesian': {
'tx': 0.0,
'ty': 0.0,
'tz': 0.0,
'rx': 0.0,
'ry': 0.0,
'rz': 0.0,
},
'pump_pwm': 0,
'valve_pwm': 0,
'motion_mode': 0,
}
# COMMUNICATION #
# send a message
def send_msg(self, msg, get_status=True):
if self.is_connected():
self.serial_device.send(msg, terminator='\r\n')
if self.debug:
print('Message sent: ', msg)
if get_status:
self.get_status()
# message receive handler
def _receive_msg(self, msg):
msg = msg.decode('utf-8')
if self.debug:
print('Message received:', msg)
if self.receive_callback is not None:
try:
self.receive_callback(msg)
except Exception as e:
print(e)
print('Receive callback error: ', sys.exc_info()[0])
if msg.startswith('<'):
self._recv_status(msg)
def _recv_status(self, msg):
pars = self.ownerComp.par
msg = msg.strip('<').strip('>')
state = msg.split(',')[0]
if state is not None:
pars.Status = state
msg = ','.join(msg.split(',')[1:])
angle = re.match(r'Angle\(ABCDXYZ\):(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),', msg)
if angle is not None:
a4, a5, a6, rail, a1, a2, a3 = angle.groups()
self.status['angle']['a1'] = float(a1)
self.status['angle']['a2'] = float(a2)
self.status['angle']['a3'] = float(a3)
self.status['angle']['a4'] = float(a4)
self.status['angle']['a5'] = float(a5)
self.status['angle']['a6'] = float(a6)
cart = re.match(r'.*Cartesian\scoordinate\(XYZ\sRxRyRz\):(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),(-*\d*.\d*),', msg)
if cart is not None:
tx, ty, tz, rx, ry, rz = cart.groups()
self.status['cartesian']['tx'] = float(tx)
self.status['cartesian']['ty'] = float(ty)
self.status['cartesian']['tz'] = float(tz)
self.status['cartesian']['rx'] = float(rx)
self.status['cartesian']['ry'] = float(ry)
self.status['cartesian']['rz'] = float(rz)
pump_pwm = re.match(r'.*Pump\sPWM:(\d+)', msg)
if pump_pwm is not None:
self.status['pump_pwm'] = int(pump_pwm.groups(0)[0])
valve_pwm = re.match(r'.*Valve\sPWM:(\d+)', msg)
if valve_pwm is not None:
self.status['valve_pwm'] = int(valve_pwm.groups(0)[0])
motion_mode = re.match(r'.*Motion_MODE:(\d+)', msg)
if motion_mode is not None:
self.status['motion_mode'] = int(motion_mode.groups(0)[0])
if self.get_status_callback is not None:
self.get_status_callback(self.status)
if self.debug:
pprint(self.status)
# check if we are connected
def is_connected(self):
return self.serial_device.is_open
# connect to the mirobot
def connect(self, portname, receive_callback=None):
self.serial_device.portname = portname
self.serial_device.baudrate = 115200
self.serial_device.stopbits = 1
self.serial_device.listen_callback = self._receive_msg
if receive_callback is not None:
self.receive_callback = receive_callback
self.serial_device.open()
# set the receive callback
def set_receive_callback(self, receive_callback):
self.receive_callback = receive_callback
# disconnect from the mirobot
def disconnect(self):
self.serial_device.close()
# COMMANDS #
# get the current status
def get_status(self, callback=None):
msg = '?'
self.get_status_callback = callback
self._send_msg(msg, get_status=False)
# home each axis individually
def home_individual(self):
msg = '$HH'
self.send_msg(msg, get_status=False)
# home all axes simultaneously
def home_simultaneous(self):
msg = '$H'
self.send_msg(msg, get_status=False)
# set the hard limit state
def set_hard_limit(self, state):
msg = '$21=' + str(int(state))
self.send_msg(msg, get_status=False)
# set the soft limit state
def set_soft_limit(self, state):
msg = '$21=' + str(int(state))
self.send_msg(msg, get_status=False)
# unlock the shaft
def unlock_shaft(self):
msg = 'M50'
self.send_msg(msg)
# send all axes to their respective zero positions
def go_to_zero(self):
self.go_to_axis(0, 0, 0, 0, 0, 0, 2000)
# send all axes to a specific position
def go_to_axis(self, a1, a2, a3, a4, a5, a6, speed):
msg = 'M21 G90'
msg += ' X' + str(a1)
msg += ' Y' + str(a2)
msg += ' Z' + str(a3)
msg += ' A' + str(a4)
msg += ' B' + str(a5)
msg += ' C' + str(a6)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# increment all axes a specified amount
def increment_axis(self, a1, a2, a3, a4, a5, a6, speed):
msg = 'M21 G91'
msg += ' X' + str(a1)
msg += ' Y' + str(a2)
msg += ' Z' + str(a3)
msg += ' A' + str(a4)
msg += ' B' + str(a5)
msg += ' C' + str(a6)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# point to point move to a cartesian position
def go_to_cartesian_ptp(self, x, y, z, a, b, c, speed):
msg = 'M20 G90 G0'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# linear move to a cartesian position
def go_to_cartesian_lin(self, x, y, z, a, b, c, speed):
msg = 'M20 G90 G1'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# point to point increment in cartesian space
def increment_cartesian_ptp(self, x, y, z, a, b, c, speed):
msg = 'M20 G91 G0'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# linear increment in cartesian space
def increment_cartesian_lin(self, x, y, z, a, b, c, speed):
msg = 'M20 G91 G1'
msg += ' X' + str(x)
msg += ' Y' + str(y)
msg += ' Z' + str(z)
msg += ' A' + str(a)
msg += ' B' + str(b)
msg += ' C' + str(c)
msg += ' F' + str(speed)
self.send_msg(msg)
return
# set the pwm of the air pump
def set_air_pump(self, pwm):
msg = 'M3S' + str(pwm)
self.send_msg(msg)
# set the pwm of the gripper
def set_gripper(self, pwm):
msg = 'M4E' + str(pwm)
self.send_msg(msg)
def __init__(self):
Base.__init__(self)
SerialDevice.__init__(self)
self.calibration = None
def findAnimaticsDevicePort(baudrate):
serialPortList = findSerialDevicePorts()
matchingList = []
for port in serialPortList:
try:
dev = SerialDevice(port=port,
baudrate=baudrate,
timeout=0.1,
write_write_delay=0.1,
write_read_delay=0.1,
debug=DEBUG)
dev.flushInput()
dev.flushOutput()
dev.writeCheckFreq(chr(MOTOR_ADDRESS_OFFSET) + 'WAKE ',
delay_write=True)
time.sleep(0.1)
dev.writeCheckFreq(chr(MOTOR_ADDRESS_OFFSET) + 'WAKE ',
delay_write=True)
time.sleep(0.1)
dev.flushInput()
dev.writeCheckFreq(chr(MOTOR_ADDRESS_OFFSET) + 'ECHO ',
delay_write=True)
dev.readlines()
response = dev.writeRead(chr(MOTOR_ADDRESS_OFFSET) + 'RPI ',
use_readline=False,
check_write_freq=True)
dev.close()
# Animatics response will either be 'RPI 3.141592741 '
# or '3.141592741 ' depending on ECHO setting
# Must enable ECHO so it will work when multiple motors are chained
response = response.replace(chr(MOTOR_ADDRESS_OFFSET) + 'RPI ', '')
if (0 < len(response)) and (response[0] == '3'):
print("Found an Animatics device!")
return port
except serial.SerialException:
pass
if len(matchingList) == 0:
err_str = 'Could not find Animatics device. Check connections and power or specify port.\n'
err_str += 'Tried ports: ' + str(serialPortList)
raise RuntimeError(err_str)
elif 1 < len(matchingList):
err_str = 'Found more than one Animatics device. Specify port.'
err_str += 'Matching ports: ' + str(matchingList)
raise RuntimeError(err_str)
else:
return matchingList[0]