Source

puma / MDC.py

Full commit
Trammell Hudson 8dd8d34 



Trammell Hudson 13b0c61 

Trammell Hudson 8dd8d34 





Trammell Hudson 13b0c61 



Trammell Hudson 6a3ec93 
Trammell Hudson 13b0c61 
Trammell Hudson e3a8f7a 

Trammell Hudson e4c0d32 
Trammell Hudson e3a8f7a 





Trammell Hudson 6a3ec93 








Trammell Hudson a6c5766 






Trammell Hudson aef2c9d 









Trammell Hudson 41b975f 
Trammell Hudson aef2c9d 

Trammell Hudson e3a8f7a 
Trammell Hudson 13b0c61 






Trammell Hudson a6c5766 
Trammell Hudson 41b975f 



Trammell Hudson 13b0c61 
Trammell Hudson b3bce6e 
Trammell Hudson a6c5766 
Trammell Hudson 8dd8d34 



Trammell Hudson e4c0d32 




















Trammell Hudson 874e48b 

Trammell Hudson e4c0d32 










Trammell Hudson 8dd8d34 



Trammell Hudson e4c0d32 


Trammell Hudson 8dd8d34 
Trammell Hudson e4c0d32 

Trammell Hudson 8dd8d34 


Trammell Hudson e4c0d32 



Trammell Hudson 8dd8d34 

Trammell Hudson 874e48b 

Trammell Hudson 8dd8d34 




















Trammell Hudson 6a3ec93 

Trammell Hudson 8dd8d34 



Trammell Hudson e4c0d32 




Trammell Hudson 8dd8d34 
Trammell Hudson e4c0d32 



Trammell Hudson 8dd8d34 
Trammell Hudson e4c0d32 






Trammell Hudson 874e48b 




Trammell Hudson 8dd8d34 




Trammell Hudson 13b0c61 

Trammell Hudson 8dd8d34 







Trammell Hudson e00e8b8 



Trammell Hudson 13b0c61 

Trammell Hudson 8dd8d34 







Trammell Hudson 874e48b 
Trammell Hudson 6a3ec93 



Trammell Hudson 8dd8d34 

Trammell Hudson e4c0d32 
Trammell Hudson 8dd8d34 




Trammell Hudson 13b0c61 
Trammell Hudson 8dd8d34 



Trammell Hudson 13b0c61 
Trammell Hudson e4c0d32 



Trammell Hudson 13b0c61 

Trammell Hudson 8dd8d34 
Trammell Hudson 13b0c61 
Trammell Hudson 8dd8d34 

Trammell Hudson 13b0c61 

Trammell Hudson 8dd8d34 

#!/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 + 1)
		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:
			if speed > 40000:
				speed = 40000
			if speed < 100:
				speed = 100
			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