puma / PumaFK.py

Diff from to

PumaFK.py

 
 from math import pi, cos, sin
 import sys
+import time
+import MDC
 
 class PumaJoint(object):
 	def __init__(self, d, a, theta, scale, coupling):
 			count = counts[i]
 			if j.coupling:
 				count -= counts[i-1] * j.coupling
-			angle = counts[i] * pi / j.scale
+			angle = counts[i] * pi / j.scale + j.theta
 			self.angles[i] = angle
 
 		c1 = cos(self.angles[0])
 	j1 = PumaJoint(0, 0, 0, -11200 - +12000, 0)
 	j2 = PumaJoint(130, 205, -90, +18000 - -18000, 0)
 	j3 = PumaJoint(0, 0, +90, -10500 - +11200, 0)
-	j4 = PumaJoint(225, 0, 0, +8000 - -8000, 0)
+	j4 = PumaJoint(225, 0, -90, +8000 - -8000, 0)
 	j5 = PumaJoint(0, 0, 0, -8000 - +8000, 9000/-40000.0)
-	j6 = PumaJoint(0, 0, 0, -6500 - +6500, 1200/-10000.0)
+	j6 = PumaJoint(50, 0, 0, -6500 - +6500, 1200/-10000.0)
 	r = PumaFK((j1,j2,j3,j4,j5,j6))
 
-	counts = [int(x) for x in sys.argv[1:]]
-	r.update(counts)
+	if len(sys.argv) == 6:
+		counts = [int(x) for x in sys.argv[1:]]
+		r.update(counts)
+		print r.p
+		sys.exit(0)
 
-	print r.p
+	#sys.exit(-1)
+	devices = ["/dev/tty.usbmodemfd1231", "/dev/tty.usbmodemfd1241", "/dev/tty.usbmodemfd1271"]
+	controllers = [MDC.MDC(dev) for dev in devices]
+
+	time.sleep(2)
+
+	while True:
+		counts = MDC.get_counts()
+		r.update([int(x) for x in counts])
+		print [int(x) for x in counts], [int(x) for x in r.p]
+		time.sleep(0.5)
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.