Commits

Anonymous committed febeae0

Add raw angle command to test angle calibrations

Comments (0)

Files changed (2)

 		if line == "a":
 			print [round(x * 180 / pi, 1) for x in r.fk.theta]
 			continue
+		g = re.match("^a (-?[0-9]+) (-?[0-9]+) (-?[0-9]+) (-?[0-9]+) (-?[0-9]+) (-?[0-9]+)", line)
+		if g is not None:
+			theta = [int(x) * pi / 180 for x in g.group(1,2,3,4,5,6)]
+			counts = r.ik.angle2count(theta, [])
+			print counts
+			r.move_raw(10000, counts)
+			continue
 		if line == "p":
 			print [int(x) for x in r.fk.p], [round(x,2) for x in r.fk.a], [round(x,2) for x in r.fk.s], [round(x,2) for x in r.fk.n]
 			continue
 		return True
 
 	def get_counts(self, current_theta):
+		return self.angle2count(self.theta, current_theta)
+
+	def angle2count(self, dest_theta, current_theta):
 		counts = []
 		for i in range(0, 6):
 			# Adjust by the home position offset to the
 			# joint-frame angle.
 			j = self.joints[i]
-			a = self.theta[i]
+			a = dest_theta[i]
 
 			# Elbow joint is off by 90 deg
 			if i == 2 and a < 0:
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.