Source

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 = {}

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

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


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)))
		self.mdc.send("!P " + self.channel + " " + str(int(count)))

class MDC:
	def __init__(self, tty):
		self.file = tty
		self.port = serial.Serial(tty, timeout=10)
		print self.file + ": Opened"
		os.system("stty < " + self.file + " icrnl")
		self.axis = -1
		self.channels = {}
		self.re = re.compile('^(.*?)=(-?[0-9]+):(-?[0-9]+)')
		self.thread = threading.Thread(target = self.startup)
		self.thread.daemon = True
		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)
		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("# 50") # report every 50 ms == 20 Hz
		time.sleep(0.1)

	def readline(self):
		line = self.port.readline().strip()
		if not line:
			return
		#print 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] = 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 and 6 < axis:
			warn(self.file + ": Channel " + str(channel) + " bad axes " + str(axis))
			return

		axis_lock.acquire()
		if axis in axises:
			warn(self.file + ": Channel " + str(channel) + " duplicate axis " + str(axis))
			axis_lock.release()
			return
		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.