Source

puma / PumaFK.py

Diff from to

PumaFK.py

 		self.s = [0,0,0]
 		self.a = [0,0,0]
 		self.p = [0,0,0]
-		self.angles = [0,0,0,0,0,0]
+		self.theta = [0,0,0,0,0,0]
 
 	def update(self, counts):
 		if len(counts) != 6:
 			if j.coupling:
 				count -= counts[i-1] * j.coupling
 			angle = counts[i] / j.scale + j.theta
-			self.angles[i] = angle
+			self.theta[i] = angle
 
-		c1 = cos(self.angles[0])
-		c2 = cos(self.angles[1])
-		c3 = cos(self.angles[2])
-		c4 = cos(self.angles[3])
-		c5 = cos(self.angles[4])
-		c6 = cos(self.angles[5])
+		c1 = cos(self.theta[0])
+		c2 = cos(self.theta[1])
+		c3 = cos(self.theta[2])
+		c4 = cos(self.theta[3])
+		c5 = cos(self.theta[4])
+		c6 = cos(self.theta[5])
 
-		s1 = sin(self.angles[0])
-		s2 = sin(self.angles[1])
-		s3 = sin(self.angles[2])
-		s4 = sin(self.angles[3])
-		s5 = sin(self.angles[4])
-		s6 = sin(self.angles[5])
+		s1 = sin(self.theta[0])
+		s2 = sin(self.theta[1])
+		s3 = sin(self.theta[2])
+		s4 = sin(self.theta[3])
+		s5 = sin(self.theta[4])
+		s6 = sin(self.theta[5])
 
-		c23 = cos(self.angles[1] + self.angles[2])
-		s23 = sin(self.angles[1] + self.angles[2])
+		c23 = cos(self.theta[1] + self.theta[2])
+		s23 = sin(self.theta[1] + self.theta[2])
 		
 		tn1 = c23*(c4*c5*c6 - s4*s6) - s23*s5*c6
 		tn2 = s4*c5*c6 + c4*s6
 	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]
+		#print [int(x) for x in counts], [int(x) for x in r.p], [round(x,3) for x in r.a], [round(x,3) for x in r.s]
+		print [int(x) for x in counts], [int(x*180/pi) for x in r.theta]
 		time.sleep(0.5)