Commits

Trammell Hudson  committed 8dcbf92

Accept floating point angles, space separated output and bind to 0.0.0.0

  • Participants
  • Parent commits 440ed10

Comments (0)

Files changed (1)

File Shuffleboard.py

 
 r = None
 
+# Map x distance to height
+# 250 -45
+# 300 -40
+# 350 -35
+# 400 -30
+# 450 -25
+
 class ShuffleboardSocket(SocketServer.StreamRequestHandler):
 	def handle(self):
+		print "New client"
 		self.alive = True
 		self.timeout = 1
 
 		# why doesn't this work as a thread?
 		self.rx_loop()
 
+	def map_z(self, x):
+		if (x >= 400 ): return -15
+		if (x >= 350 ): return -25
+		if (x >= 300 ): return -30
+		if (x >= 250 ): return -35
+		if (x >= 200 ): return -40
+		return 0
+
 	def moveto(self,x,angle):
 		# Compute approach vector based on the angle
 		# For the swinging tool on the left side of the board,
 		n = [-sin(angle), cos(angle), 0]
 
 		# todo: Compute the Z position based on the X distance
-		r.move_ik(5000, [x,130,0], a, s, n)
+		z = self.map_z(x)
+		r.move_ik(5000, [x,130,z], a, s, n)
 
 	def rx_loop(self):
 		while self.alive:
 				r.hold()
 				continue
 
-			g = re.match("^move ([0-9]+) (-?[0-9]+)", line)
+			g = re.match("^move ([0-9]+) (-?[0-9.]+)", line)
 			if g is not None:
-				self.moveto(int(g.group(1)), int(g.group(2)))
+				self.moveto(int(g.group(1)), float(g.group(2)))
 				continue
 			g = re.match("^swing ([0-9]+)", line)
 			if g is not None:
 				continue
 
 			self.wfile.write("?\r\n")
+		self.alive = False
+		print "Client exited"
 
 	def tx_loop(self):
 		while self.alive:
 			volts = r.volts
 			pos = [str(round(x,2)) for x in r.fk.p]
 			speed = str(r.counts[5])
-			self.wfile.write(str(round(volts,1)) + ',' + ','.join(pos) + ',' + speed + '\n')
+			self.wfile.write(str(round(volts,1)) + ' ' + ' '.join(pos) + ' ' + speed + '\n')
 			time.sleep(0.5)
 
 
 	allow_reuse_address = True
 
 if __name__ == "__main__":
-	host = 'localhost'
+	host = '0.0.0.0'
 	port = 27189
 	server = ServerThread((host,port), ShuffleboardSocket)