puma / MDC.py

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

axis_lock = threading.Lock()
axises = {}
speeds = {}
counts = {}
amps = {}
volts = {}

def get_counts():
	s = []
	for axis in range(1,6+1):
		if axis not in counts:
			s.append('?')
		else:
			s.append(str(counts[axis]))
	return s

def get_volts():
	s = []
	for axis in range(1,6+1):
		if axis not in volts:
			s.append('?')
		else:
			s.append(str(volts[axis]))
	return s

def moveto(axis,pos,vel):
	if axis not in axises:
		return False
	chan = axises[axis]
	chan.moveto(pos, vel)
	return True

def move_all(ms, dest):
	counts = get_counts()
	for i in range(0,6):
		if counts[i] == 'X':
			continue
		new_pos = dest[i]
		delta = int(counts[i]) - new_pos
		if delta < 0:
			delta = -delta
		if ms != 0:
			moveto(i+1, new_pos, delta * 1000 / ms)
		else:
			moveto(i+1, new_pos, 0)

class MDC_channel:
	def __init__(self, mdc, channel, axis):
		self.mdc = mdc
		self.channel = str(channel)
		self.axis = axis

	def moveto(self, count, speed=0):
		if speed is not None and speed != 0:
			self.mdc.send("!S " + self.channel + " " + str(int(speed)))
		print str(self.axis) + ": " + str(int(count)) + " @ " + str(int(speed))
		self.mdc.send("!P " + self.channel + " " + str(int(count)))

class MDC:
	def __init__(self, tty):
		self.file = tty
		self.port = None
		self.axis = -1
		self.channels = {}
		self.timeout = 1
		self.last_time = time.time()
		self.re = re.compile('^(.*?)=(-?[0-9]+):(-?[0-9]+)')
		self.thread = threading.Thread(target = self.readloop)
		self.thread.daemon = True
		self.thread.start()

	def open(self):
		if self.port is not None:
			self.port.close()
		try:
			self.port = serial.Serial(self.file, timeout=1)
		except serial.SerialException:
			self.port = None
			print self.file + ": Failed to open (sleeping)"
			time.sleep(5)
			return False

		print self.file + ": Opened"
		os.system("stty < " + self.file + " icrnl")
		return True

	def reset(self):
		if 1 in self.channels:
			c1 = self.channels[1]
			counts[c1] = 'X'
		if 2 in self.channels:
			c2 = self.channels[2]
			counts[c2] = 'X'
		self.port.close()
		self.port = None

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

	# Read lines continuously.  If readline ever fails,
	# close and re-open the serial device.
	def readloop(self):
		while True:
			if not self.port:
				self.init()
			self.readline()

	def init(self):
		if not self.open():
			return

		print self.file + ": Reinit"
		self.send("# C")
		time.sleep(0.1)
		self.send("~ACTR")
		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("?V") # volts
		time.sleep(0.1)
		self.send("# 50") # report every 50 ms == 20 Hz
		time.sleep(0.1)

	def readline(self):
		if not self.port:
			return

		line = self.port.readline()
		now = time.time()
		if not line:
			if now - self.last_time < self.timeout:
				return
			print time.ctime() + ":" + self.file + ": timeout " + str(now) + " - " + str(self.last_time)
			self.reset()
			return

		self.last_time = now
		line = line.strip()

		if line == "":
			return
		#print time.ctime() + ":" + self.file + ": read '" + line + "'"

		# ignore echoed commands
		if re.match("^[!^~#?+]", line):
			return

		m = self.re.match(line)
		if m is None:
			warn(self.file + ": Unable to parse '" + line + "'")
			return
		type = m.group(1)
		x1 = int(m.group(2))
		x2 = int(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
		if 1 not in self.channels:
			return
		if 2 not in self.channels:
			return
		c1 = self.channels[1]
		c2 = self.channels[2]
		
		if type == 'C':
			counts[c1] = x1
			counts[c2] = x2
		elif type == 'A':
			amps[c1] = x1
			amps[c2] = x2
		elif type == 'S':
			speeds[c1] = x1
			speeds[c2] = x2
		elif type == 'V':
			volts[c1] = x1
			volts[c2] = x2
		else:
			warn(self.file + ": Unknown type '" + type + "'")


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

		axis_lock.acquire()
		if axis in axises:
			chan = axises[axis]
			if chan.mdc != self and chan.channel != channel:
				warn(self.file + ": Channel " + str(channel) + " duplicate axis " + str(axis))

		axises[axis] = MDC_channel(self, channel, axis)
		self.channels[channel] = axis
		axis_lock.release()
		print(self.file + ": Axis " + str(axis) + ": channel " + str(channel))

if __name__ == "__main__":
	#c = MDC("/dev/tty.usbmodem12341")
	c = MDC("/dev/ttys005")
	while True:
		time.sleep(1)
		print counts
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.