Commits

Trammell Hudson committed edeff5a

Add logging support

Comments (0)

Files changed (2)

 from warnings import warn
 
 axis_lock = threading.Lock()
+axis_count = 0
 axises = {}
 speeds = {}
 counts = {}
 amps = {}
 volts = {}
+powers = {}
+commands = {
+	1: 0,
+	2: 0,
+	3: 0,
+	4: 0,
+	5: 0,
+	6: 0,
+}
 
 def get_counts():
 	s = []
 			self.mdc.send("!S " + self.channel + " " + str(int(speed)))
 		print str(self.axis) + ": " + str(int(count)) + " @ " + str(int(speed))
 		self.mdc.send("!P " + self.channel + " " + str(int(count)))
+		commands[int(self.axis)] = count
 
 class MDC:
 	def __init__(self, tty):
 			time.sleep(0.1)
 			self.send("^KD " + channel + " 5")
 			time.sleep(0.1)
+			self.send("^KI " + channel + " 1")
+			time.sleep(0.1)
 			self.send("!s " + channel + " 4000")
 			time.sleep(0.1)
-			self.send("!ac " + channel + " 50000")
+			self.send("!ac " + channel + " 60000")
 			time.sleep(0.1)
 			self.send("!dc " + channel + " 60000")
 			time.sleep(0.1)
 		time.sleep(0.1)
 		self.send("?C") # counts
 		time.sleep(0.1)
-		#self.send("?M") # motor command
-		#time.sleep(0.1)
+		self.send("?P") # motor command
+		time.sleep(0.1)
 		self.send("?S") # speed
 		time.sleep(0.1)
 		self.send("?V") # volts
 		elif type == 'V':
 			volts[c1] = x1
 			volts[c2] = x2
+		elif type == 'P':
+			powers[c1] = x1
+			powers[c2] = x2
+			#print "CMD " + str(c1) + ": " + str(x1) + " , " + str(x2)
 		else:
 			warn(self.file + ": Unknown type '" + type + "'")
 
 			warn(self.file + ": Channel " + str(channel) + " bad axes " + str(axis))
 			return
 
+		global axis_count
 		axis_lock.acquire()
 		if axis in axises:
 			chan = axises[axis]
 
 		axises[axis] = MDC_channel(self, channel, axis)
 		self.channels[channel] = axis
+		axis_count += 1
 		axis_lock.release()
 		print(self.file + ": Axis " + str(axis) + ": channel " + str(channel))
 
 			line = self.rfile.readline().strip()
 			if not line:
 				break
-			print "Read line '" + line + "'"
+			#print "Read line '" + line + "'"
 
 			if line == "save":
 				counts = MDC.get_counts()
 			self.wfile.write(','.join(counts) + '\n')
 			time.sleep(0.1)
 
+def write_log():
+	f = open("robot.csv", "w")
+	f.write("t,c1,p1,d1,c2,p2,d2,c3,p3,d3,c4,p4,d4,c5,p5,d5,c6,p6,d6\n")
+	t = 0
+
+	while MDC.axis_count != 6:
+		pass
+
+	time.sleep(2)
+	print MDC.counts
+
+	while True:
+		s = str(t)
+		t += 1
+		for i in range(1,6+1):
+			s += "," + str(MDC.counts[i])
+			s += "," + str(MDC.powers[i])
+			s += "," + str(MDC.commands[i])
+		f.write(s + "\n")
+		time.sleep(0.1)
+
+
 class ServerThread(SocketServer.ThreadingMixIn, SocketServer.TCPServer):
 	# Force SOREUSEADDR allways
 	daemon_threads = True
 	devices = ["/dev/tty.usbmodemfd1231", "/dev/tty.usbmodemfd1241", "/dev/tty.usbmodemfd1271"]
 	controllers = [MDC.MDC(dev) for dev in devices]
 
+	log_thread = threading.Thread(target = write_log)
+	log_thread.daemon = True
+	log_thread.start()
+
 	host = 'localhost'
 	port = 31415
 	server = ServerThread((host,port), MDCSocket)