Source

puma / MDC.py

Full commit
#!/usr/bin/python
# Driver for the RobotEQ MDC motor controllers.
# Exports an array of speeds, counts and amps.
#
import serial
import threading
import re
import time
from warnings import warn

axis_lock = threading.Lock()
axises = []
speeds = []
counts = []
amps = []

class MDC:
	def __init__(self, tty):
		self.file = tty
		self.port = serial.Serial(tty, timeout=1)
		self.axis = -1
		self.channels = []
		self.re = re.compile('(.*?)=([0-9]+):([0-9]+)')
		self.thread = threading.Thread(target = self.startup)
		self.thread.start()

	def send(self, str):
		self.port.write(str + "\r\n")

	def startup(self):
		self.init()
		while True:
			self.readline()

	def init(self):
		self.send("# C")
		time.sleep(0.1)

		# Configure each channel's speed and acceleration
		for channel in ['1','2']:
			self.send("^CLERD " + channel + " 0")
			time.sleep(0.1)
			self.send("!s " + channel + " 4000")
			time.sleep(0.1)
			self.send("!ac " + channel + " 40000")
			time.sleep(0.1)
			self.send("!dc " + channel + " 40000")
			time.sleep(0.1)

		# Configure the telemetry stream
		self.send("# C")  # clear
		time.sleep(0.1)
		self.send("?C") # counts
		time.sleep(0.1)
		#self.send("?M") # motor command
		#time.sleep(0.1)
		self.send("?S") # speed
		time.sleep(0.1)
		self.send("# 50") # report every 50 ms == 20 Hz
		time.sleep(0.1)

	def readline(self):
		line = self.port.readline().strip()
		if not line:
			return
		m = self.re.match(line)
		if m is None:
			warn(self.file + ": Unable to parse '" + line + "'")
			return
		type = m.group(1)
		x1 = m.group(2)
		x2 = m.group(3)

		if type == 'ACTR':
			self.assign_axis(1, x1)
			self.assign_axis(2, x2)
			return

		# if we haven't had the axes assigned, ignore all
		# the data from the serial port
		c1 = self.channel[1]
		c2 = self.channel[2]
		if not c1 or not c2:
			return
		
		if type == 'C':
			counts[c1] = x1
			counts[c2] = x2
		elif type == 'A':
			amps[c1] = x1
			amps[c2] = x2
		elif type == 'S':
			speed[c1] = x1
			speed[c2] = x1
		else:
			warn(self.file + ": Unknown type '" + type + "'")
			return

	#
	# Assign a channel on this controller to a given axis
	#
	def assign_axis(self, channel, axis):
		if axis < 1 or 6 < axis or axis2 < 1 or 6 < axis2:
			warn(self.file + ": Channel " + str(channel) + " bad axes " + str(axis))
			return

		axis_lock.acquire()
		if axises[axis]:
			warn(self.file + ": Channel " + str(channel) + " duplicate axis " + str(axis))
			axis_lock.release()
			return
		axises[axis] = [self, channel]
		self.channel[channel] = axis
		axis_lock.release()

if __name__ == "__main__":
	c = MDC("/dev/tty.usbmodem12341")
	while True:
		time.sleep(1)
		print counts