Trammell Hudson avatar Trammell Hudson committed b3bce6e

coordinated goto seems to work in the server

Comments (0)

Files changed (3)

 	def moveto(self, count, speed=0):
 		if speed is not None and speed != 0:
 			self.mdc.send("!S " + self.channel + " " + str(int(speed)))
-		print str(self.axis) + ": " + str(int(count))
+		print str(self.axis) + ": " + str(int(count)) + " @ " + str(int(speed))
 		self.mdc.send("!P " + self.channel + " " + str(int(count)))
 
 class MDC:
 {
 	my $speed = shift;
 
+	print "go,$speed,", join(",", @_), "\n";
+	return;
+
 	# If there is an old position, compute the delta
 	my $max_delta = 0;
 
 home => [ 0,0,0,0,0,0],
 );
 
-my $speed = 200;
+my $speed = 20000;
 
 while(<>)
 {
 
 class MDCSocket(SocketServer.StreamRequestHandler):
 	def handle(self):
-		self.re = re.compile('^([0-9]),(-?[0-9]+)(?:,([0-9]+))?$')
 		self.alive = True
 		self.timeout = 1
 
 					MDC.moveto(i+1, 0, 500)
 				continue
 
-			m = self.re.match(line)
+			if line.startswith("go,"):
+				if not self.go(line):
+					self.wfile.write("! go error\n")
+				continue
+
+			m = re.match('^([0-9]),(-?[0-9]+)(?:,([0-9]+))?$', line)
 			if m is None:
 				self.wfile.write("! parse error\n")
 				continue
 
 		self.alive = False
 		print "Client exited"
-	
+
+	# Parse a "go" command with the six joint positions:
+	# go,ms,1,2,3,4,5,6
+	def go(self,line):
+		args = line.split(",")
+		if len(args) != 8:
+			return False
+		ms = int(args[1])
+		counts = MDC.get_counts()
+		for i in range(0,6):
+			if counts[i] == 'X':
+				continue
+			new_pos = int(args[i+2])
+			delta = int(counts[i]) - new_pos
+			if delta < 0:
+				delta = -delta
+			if ms != 0:
+				MDC.moveto(i+1, new_pos, delta * 1000 / ms)
+			else:
+				MDC.moveto(i+1, new_pos, 0)
+		return True
+
 	def tx_loop(self):
 		while self.alive:
 			#counts = MDC.get_volts()
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.