Commits

Trammell Hudson committed 7c4b3b0

Puma class talks to MDC server

Comments (0)

Files changed (1)

+#!/usr/bin/python
+#
+# Puma robot arm class.
+# Talks to the MDC network socket to get the state of the
+# controllers.  Maps to FK position, commands via IK, etc.
+#
+from PumaFK import PumaFK
+from PumaIK import PumaIK
+import socket
+import threading
+import time
+
+class Puma:
+	def __init__(self, con, joints):
+		self.live = True
+		self.joints = joints
+		self.counts = [0,0,0,0,0]
+		self.fk = PumaFK(joints)
+		self.ik = PumaIK(True, True, joints)
+		self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+		self.sock.connect(con)
+		self.con = self.sock.makefile()
+		self.thread = threading.Thread(target = self.rx_loop)
+		self.thread.daemon = True
+		self.thread.start()
+
+	def rx_loop(self):
+		while True:
+			line = self.con.readline()
+			if not line:
+				break
+			line = line.strip()
+			counts = line.split(",")
+			if len(counts) != 6:
+				print "Parse error: " + line
+				continue
+			#print "rx: " + line
+			self.fk.update([int(x) for x in counts])
+
+		print "RX THREAD EXITED"
+		self.live = False
+
+if __name__ == "__main__":
+	import PumaConfig
+	r = Puma(("localhost", 31415), PumaConfig.joints)
+
+	while r.live:
+		print r.fk.p
+		time.sleep(1)