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.