Commits

Elias Bakken committed fe6e548

Too late to test this, but I think I just found the bug that has been causing the print to stop when wrapping around the DDR.

Comments (0)

Files changed (7)

firmware/firmware_pru_0.p

 	MOV  r16, GPIO3_MASK						// Make the mask
     MOV  r17, GPIO3 | GPIO_DATAOUT				// Load address
     MOV  r18, 0xFFFFFFFF ^ (GPIO3_MASK)			// Invert mask
-
+	
+	MOV  r0, 4									// Load the address of the events_counter 
+	LBBO r6, r0, 0, 4							// Put it in R6
+	MOV  r5, 0									// Make r5 the nr of events counter
+	SBBO r5, r6, 0, 4							// store the number of interrupts that have occured in the second reg of DRAM
+	
 RESET_R4:
 	MOV  r0, 0
 	LBBO r4, r0, 0, 4							// Load the ddr_addr from the first adress in the PRU0 DRAM
 
 BLINK:
 	ADD  r4, r4, 4								// Increment r4
+
     LBBO r2, r4, 0, 4							// Load pin data into r2
-	AND  r2, r2, r10							// Mask the pins to PRU0, GPIO1
+	AND  r2, r2, r10							// Mask the pins to GPIO1
 	LBBO r3, r11, 0, 4							// Load the data currently in addr r3
 	AND	 r3, r3, r12							// Mask the data so only the necessary pins can change
 	OR   r3, r3, r2 							// Add the GPIO1-mask to hinder toggling PRU1's pins
     SBBO r3, r11, 0, 4							// Ok, set the pins
 
     LBBO r2, r4, 0, 4							// Load pin data into r2
-	AND  r2, r2, r13							// Mask the pins to PRU0, GPIO1
+	AND  r2, r2, r13							// Mask the pins to GPIO2
 	LBBO r3, r14, 0, 4							// Load the data currently in addr r3
 	AND	 r3, r3, r15							// Mask the data so only the necessary pins can change
-	OR   r3, r3, r2 							// Add the GPIO1-mask to hinder toggling PRU1's pins
+	OR   r3, r3, r2 							// Add the GPIO2-mask to hinder toggling PRU1's pins
     SBBO r3, r14, 0, 4							// Ok, set the pins
 
     LBBO r2, r4, 0, 4							// Load pin data into r2
-	AND  r2, r2, r16							// Mask the pins to PRU0, GPIO1
+	AND  r2, r2, r16							// Mask the pins to GPIO3
 	LBBO r3, r17, 0, 4							// Load the data currently in addr r3
 	AND	 r3, r3, r18							// Mask the data so only the necessary pins can change
 	OR   r3, r3, r2 							// Add the GPIO1-mask to hinder toggling PRU1's pins
     QBNE BLINK, r1, 0							// Still more pins to go, jump back
 	ADD  r4, r4, 4			
 
+	ADD r5, r5, 1								// r5++
+	SBBO r5, r6, 0, 4							// store the number of interrupts that have occured in the second reg of DRAM
     MOV R31.b0, PRU0_ARM_INTERRUPT+16   		// Send notification to Host that the instructions are done
 
 	MOV  r3, DDR_MAGIC							// Load the fancy word into r3
         
     ''' Get the length of this path segment '''
     def get_length(self):      
-        if not "X" in self.axes or not "Y" in self.axes:
-            return 0.0
-        x = self.axes["X"]/1000.0
-        y = self.axes["Y"]/1000.0
+        if "X" in self.axes:
+            x = self.axes["X"]/1000.0
+            if self.movement == "ABSOLUTE":
+                x -= self.global_pos["X"]
+        else:
+            x = 0
+        if "Y" in self.axes:
+            y = self.axes["Y"]/1000.0
+            if self.movement == "ABSOLUTE":        
+                y -= self.global_pos["Y"]
+        else:
+            y = 0
 
-        if self.movement == "ABSOLUTE":
-            x -= self.global_pos["X"]
-            y -= self.global_pos["Y"]
         self.length = np.sqrt(x**2+y**2)                             # calculate the hypotenuse to the X-Y vectors, 
 
         if "Z" in self.axes:
         hyp     = self.get_length()    	                                # Calculate the ratio               
         if hyp == 0.0:
             return 1.0
-        return self.get_axis_length(axis)/hyp
+        return abs(self.get_axis_length(axis))/hyp
 
     ''' Get the lowest speed along this segment '''
     def get_min_speed(self):

software/Path_planner.py

 '''
 
 import time
+import logging
 import numpy as np  
 from threading import Thread
 from Pru import Pru
+import Queue
 
 class Path_planner:
     ''' Init the planner '''
-    def __init__(self, steppers):
+    def __init__(self, steppers, current_pos):
         self.steppers    = steppers
         self.pru         = Pru()                                # Make the PRU
-        self.paths       = list()                               # Make a list of paths
-        self.current_pos = {"X":0.0, "Y":0.0, "Z":0.0, "E":0.0} # Current position in (x, y, z, e)
+        self.paths       = Queue.Queue(100)                      # Make a queue of paths
+        self.current_pos = current_pos                          # Current position in (x, y, z, e)
         self.running     = True                                 # Yes, we are running
         self.t           = Thread(target=self._do_work)         # Make the thread
         self.t.start()		                
 
     ''' Add a path segment to the path planner '''        
     def add_path(self, new):        
-        self.paths.append(new)
-        if len(self.paths) > 1:
-            prev = self.paths[0]
-            new.set_prev(prev)
-            prev.set_next(new)
+        self.paths.put(new)
         
     ''' Return the number of paths currently on queue '''
     def nr_of_paths(self):
-        return len(self.paths)
-
-    ''' Join the thread '''
-    def exit(self):
-        self.running = False
-        self.pru.exit()
-        self.t.join()
-
-    def set_pos(self, axis, pos):
-        self.current_pos[axis] = pos
-    
-    def get_pos(self, axis):
-        return self.current_pos[axis]
-
-    def reset_pos(self):
-        self.current_pos = {"X":0.0, "Y":0.0, "Z":0.0, "E":0.0}
-        print "Path planner: pos reset"
-    
-    ''' Add a certain length to a vector '''
-    def add_to_pos(axis, vec):
-        self.current_pos[axis] += vec
+        return self.paths.qsize()
 
+    ''' Set position for an axis '''
+    def set_pos(self, axis, val):
+        self.current_pos[axis] = val
+ 
     ''' This loop pops a path, sends it to the PRU 
     and waits for an event '''
     def _do_work(self):
         events_waiting = 0
-        while self.running:
-            if len(self.paths) > 0:
-                path = self.paths.pop(0)                            # Get the last path added
-                path.set_global_pos(self.current_pos.copy())               # Set the global position of the printer
+        while self.running:       
+            try: 
+                path = self.paths.get(timeout = 1)                            # Get the last path added
+                path.set_global_pos(self.current_pos.copy())       # Set the global position of the printer
                 axes_added = 0
                 all_data = {}
                 slowest =  0
-                for axis in path.get_axes():                        # Run through all the axes in the path    
-                    stepper = self.steppers[axis]                   # Get a handle of  the stepper                    
+                for axis in path.get_axes():                       # Run through all the axes in the path    
+                    stepper = self.steppers[axis]                  # Get a handle of  the stepper                    
                     data = self._make_data(path, axis)
                     if len(data[0]) > 0:
-                        all_data[axis] = data                       # Generate the timing and pin data                         
+                        all_data[axis] = data                      # Generate the timing and pin data                         
                         slowest = max(slowest, sum(data[1]))   
-                
+                                
+
                 for axis in all_data:                         
                     packet = all_data[axis]                           
                     delays = np.array(packet[1])
+                    #print "Axis "+axis+" uses "+str(sum(delays))
                     diff = (slowest-sum(delays))/len(delays)
                     for j, delay in enumerate(delays):
-                        delays[j] = max(delay+diff, 2.0/1000.0)   # min 0.2ms                     
-                    data = (packet[0], delays)                    
-                    while not self.pru.has_capacity_for(len(delays)*8):# Wait until the PRU has capacity for this chunk of data
-                        time.sleep(1)
-                        print "no capacity: "+str(self.pru.get_capacity())
-                    axes_added += self.pru.add_data(data)
+                        delays[j] = max(delay+diff, 1.0/10000.0)    # min 0.2ms                     
+                    data = (packet[0], delays)  
+                    #print "Axis "+axis+" uses "+str(sum(delays))
+                
+                if "Z" in all_data:     # HACK! The Z-axis cannot be combined with the other data
+                    packet = all_data["Z"]      
+                    while not self.pru.has_capacity_for(len(packet[0])*8):# Wait until the PRU has capacity for this chunk of data
+                        print "PRU does not have capacity for "+str(len(packet[0])*8),
+                        print "only has "+str(self.pru.get_capacity())
+                        time.sleep(1)                   
+                    if self.pru.add_data(packet) > 0:                        
+                        self.pru.commit_data() 
+                    del all_data["Z"]
+                    
+                for axis in all_data:   # Commit the other axes    
+                    packet = all_data[axis]
+                    while not self.pru.has_capacity_for(len(packet[0])*8):# Wait until the PRU has capacity for this chunk of data
+                        print "PRU does not have capacity for "+str(len(packet[0])*8),
+                        print "only has "+str(self.pru.get_capacity())
+                        time.sleep(1)                   
+                    axes_added += self.pru.add_data(packet)
                     
-
                 if axes_added > 0:
                     self.pru.commit_data()                            # Commit data to ddr
-                    
-                    #self.pru.autoclear_event()
-                '''
-                for axis in all_data: 
-                    packet = all_data[axis]                           
-                    delays = np.array(packet[1])
-                    diff = (slowest-sum(delays))/len(delays)
-                    for j, delay in enumerate(delays):
-                        delays[j] = max(delay+diff, 2.0/10000)                     
-                    data = (packet[0], delays)                    
-                    self.steppers[axis].add_data(data)
-
-                for axis in all_data:                            
-                    self.steppers[axis].prepare_move()
-                for axis in all_data:                            
-                    self.steppers[axis].start_move()
-                for axis in all_data:                            
-                    self.steppers[axis].end_move()               
-                '''                         
-            else:
-                time.sleep(1)                                    # If there is no paths to execute, sleep. 
-                #print "Path planner: Que empty"
+                                     
+                self.paths.task_done()
+               
+            except Queue.Empty:
+                pass
+    ''' Join the thread '''
+    def exit(self):
+        self.running = False
+        self.pru.join()
+        logging.debug("pru joined")
+        self.t.join()
+        logging.debug("path planner joined")
+
 
     ''' Make the data for the PRU or steppers '''
     def _make_data(self, path, axis):     
         distances       = list(np.arange(0, sm, ds))		            # Table of distances                       
         timestamps      = [(-u+np.sqrt(2.0*a*ss+u*u))/a for ss in distances]# Make a table of times when a tick occurs   
         delays          = np.diff(timestamps)/2.0			                # We are more interested in the delays pr second.         
+        #logging.info(axis+" Ramp single uses "+str(sum(delays)))
         delays = list(np.array([delays, delays]).transpose().flatten()) # Double the array so we have timings for up and down  
-  
+        #logging.info(axis+" Ramp uses "+str(sum(delays)))
+
+        #logging.info("ratio: "+str(ratio))
+        #logging.info("ds: "+str(ds))
+        #logging.info("Vm: "+str(Vm))
+
         i_steps     = num_steps-len(delays)		                        # Find out how many delays are missing
         i_delays    = [(ds/Vm)/2.0]*i_steps*2		                    # Make the intermediate steps
         delays      += i_delays+delays[::-1]                            # Add the missing delays. These are max_speed
         path.set_travelled_distance(axis, td)                           # Set the travelled distance back in the path 
         self.current_pos[axis] += td                                    # Update the global position vector
 
+        #logging.info(axis+" uses "+str(sum(delays)))
         return (pins, delays)                                           # return the pin states and the data
 
 
 
 
+'''
+for axis in all_data: 
+    packet = all_data[axis]                           
+    delays = np.array(packet[1])
+    diff = (slowest-sum(delays))/len(delays)
+    for j, delay in enumerate(delays):
+        delays[j] = max(delay+diff, 2.0/10000)                     
+    data = (packet[0], delays)                    
+    self.steppers[axis].add_data(data)
+
+for axis in all_data:                            
+    self.steppers[axis].prepare_move()
+for axis in all_data:                            
+    self.steppers[axis].start_move()
+for axis in all_data:                            
+    self.steppers[axis].end_move()               
+'''                         
+
+
 
 import os
 os.system("export LD_LIBRARY_PATH=/usr/local/lib")
+import logging
 import pypruss      					                            # The Programmable Realtime Unit Library
 import numpy as np						                            # Needed for braiding the pins with the delays
-from threading import Thread
+from threading import Thread, Lock
+import Queue
 import time 
 import mmap
 import struct 
         self.inst_pr_loop 	= 16                        # This is the minimum number of instructions needed to step. 
         self.inst_pr_delay 	= 2                         # Every loop adds two instructions: i-- and i != 0            
         self.pru_data       = []      	    	        # This holds all data for one move (x,y,z,e1,e2)
-        self.ddr_used       = []                        # List of data lengths currently in DDR for execution
-        self.ddr_reserved   = 0        
-        self.clear_events   = []
+        self.ddr_used       = Queue.Queue(100)           # List of data lengths currently in DDR for execution
+        self.ddr_reserved   = 0      
+        self.ddr_mem_used   = 0  
+        self.clear_events   = []       
+        self.ddr_lock       = Lock() 
 
+        self.i = 0
         pypruss.modprobe(0x40000)    			        # This only has to be called once pr boot
         self.ddr_addr = pypruss.ddr_addr()
-        self.ddr_size = pypruss.ddr_size()              
+        self.ddr_size = pypruss.ddr_size() 
         print "The DDR memory reserved for the PRU is "+hex(self.ddr_size)+" and has addr "+hex(self.ddr_addr)
 
         ddr_offset     		= self.ddr_addr-0x10000000  # The Python mmap function cannot accept unsigned longs. 
         self.DDR_START      = 0x10000000
         self.DDR_END        = 0x10000000+self.ddr_size
         self.ddr_start      = self.DDR_START
+        self.ddr_nr_events  = self.ddr_addr+self.ddr_size-4
+
 
         with open("/dev/mem", "r+b") as f:	            # Open the memory device
             self.ddr_mem = mmap.mmap(f.fileno(), ddr_filelen, offset=ddr_offset) # mmap the right area            
         pypruss.init()						            # Init the PRU
         pypruss.open(0)						            # Open PRU event 0 which is PRU0_ARM_INTERRUPT
         pypruss.pruintc_init()					        # Init the interrupt controller
-        pypruss.pru_write_memory(0, 0, [self.ddr_addr])		# Put the ddr address in the first region 
+        pypruss.pru_write_memory(0, 0, [self.ddr_addr, self.ddr_nr_events])		# Put the ddr address in the first region 
         pypruss.exec_program(0, "../firmware/firmware_pru_0.bin")	# Load firmware "ddr_write.bin" on PRU 0
         self.t = Thread(target=self._wait_for_events)         # Make the thread
         self.running = True
         self.t.start()		                
-        print "PRU initialized"
+
+        logging.debug("PRU initialized")
 
     ''' Add some data to one of the PRUs '''
     def add_data(self, data):
 
     ''' Check if the PRU has capacity for a chunk of data '''
     def has_capacity_for(self, data_len):
-        cap = self.ddr_size-sum(self.ddr_used)-self.ddr_reserved
-        return (cap > data_len)
-    
+        with self.ddr_lock:
+            cap = self.ddr_size-self.ddr_mem_used
+        return (cap > data_len) 
+
+    ''' Check if the PRU has capacity for a chunk of data '''
     def get_capacity(self):
-         return self.ddr_size-sum(self.ddr_used)-self.ddr_reserved
-
-    ''' Catch events on queue '''
-    def _do_work(self):
-        while self.running:            
-            if len(self.ddr_used) > 0:
-                ev = self.clear_events.pop(0)
-                self.wait_for_event(ev)
-                #print "Cleared event "+str(ev)+" cap is "+str(self.ddr_size-sum(self.ddr_used)-self.ddr_reserved)
-                #print "reserved is "+str(self.ddr_reserved)
-            else:
-                time.sleep(0.1)
-    ''' Join the thread '''
-    def exit(self):
-        self.running = False
-        self.t.join()   
+        with self.ddr_lock:
+            cap = self.ddr_size-self.ddr_mem_used
+        return cap
 
     ''' Commit the data to the DDR memory '''
     def commit_data(self):
         data += struct.pack('L', 0)                             # Add a terminating 0, this keeps the fw waiting for a new command.
 
         self.ddr_end = self.ddr_start+len(data)       
-        if self.ddr_end > self.DDR_END:                         # If the data is too long, wrap it around to the start
+        if self.ddr_end >= self.DDR_END-16:                        # If the data is too long, wrap it around to the start
             multiple = (self.DDR_END-self.ddr_start)%8          # Find a multiple of 8
-            cut = self.DDR_END-self.ddr_start-multiple-4        # The cut must be done after a delay, so a multiple of 8 bytes +/-4
-
-            first = struct.pack('L', cut/8)+data[4:cut]         # Update the loop count
-            first += struct.pack('L', DDR_MAGIC)                # Add the magic number to force a reset of DDR memory counter
+            cut = self.DDR_END-self.ddr_start-multiple-4-8      # The cut must be done after a delay, so a multiple of 8 bytes +/-4
+        
+            first = struct.pack('L', len(data[4:cut])/8)+data[4:cut]    # Update the loop count
+            first += struct.pack('L', DDR_MAGIC)            # Add the magic number to force a reset of DDR memory counter
+            print "Laying out from "+hex(self.ddr_start)+" to "+hex(self.ddr_start+len(first))
             self.ddr_mem[self.ddr_start:self.ddr_start+len(first)] = first  # Write the first part of the data to the DDR memory.
-            self.ddr_used.append(len(first))
-            self.ddr_reserved -= len(first)
 
-            second = struct.pack('L', (len(data[cut:-4])/8))+data[cut:]     # Add the number of steps in this iteration
-            self.ddr_end = self.DDR_START+len(second)           # Update the end counter
-            self.ddr_mem[self.DDR_START:self.ddr_end] = second  # Write the second half of data to the DDR memory.
+            with self.ddr_lock:
+                self.ddr_mem_used += len(first)
+            self.ddr_used.put(len(first))
+            self.ddr_reserved -= len(first)
 
-            print "Wrapped"
+            if len(data[cut:-4]) > 0:                                 # If len(data) == 4, only the terminating zero is present..
+                second = struct.pack('L', (len(data[cut:-4])/8))+data[cut:]     # Add the number of steps in this iteration
+                self.ddr_end = self.DDR_START+len(second)           # Update the end counter
+                print "Second batch starts from "+hex(self.DDR_START)+" to "+hex(self.ddr_end)
+                print "First register is "+hex(struct.unpack("L", second[0:4])[0])
+                print "Last register is "+hex(struct.unpack("L", second[-4::])[0])
+                self.ddr_mem[self.DDR_START:self.ddr_end] = second  # Write the second half of data to the DDR memory.
+            else:
+                 self.ddr_end = self.DDR_START
+                 self.ddr_mem[self.DDR_START:4] = struct.pack('L', 0) # Terminate the first word
+                print "Second batch skipped, 0 length"
+            
+            print "Wrapped. Capacity is now "+str(self.get_capacity())
         else:
             self.ddr_mem[self.ddr_start:self.ddr_end] = data    # Write the data to the DDR memory.
-        
-        print "Appending "+str(self.ddr_reserved), 
-        print " waiting "+str(len(self.ddr_used))
-        self.ddr_used.append(self.ddr_reserved) 		        # update the amount of memory used 
-        self.ddr_reserved   = 0
-        self.ddr_start 		= self.ddr_end-4                   # Update the start of ddr for next time 
-        self.pru_data 		= []                               # Reset the pru_data list since it has been commited         
 
+        self.ddr_start 		= self.ddr_end-4                    # Update the start of ddr for next time 
+        with self.ddr_lock:
+            self.ddr_mem_used += self.ddr_reserved
+            #print "Pushed "+str(self.ddr_reserved)+ "\tnow "+str(self.ddr_used.qsize())
+        self.ddr_used.put(self.ddr_reserved) 		            # update the amount of memory used 
+        self.ddr_reserved   = 0        
+        self.pru_data 		= []                                # Reset the pru_data list since it has been commited         
 
-    ''' Wait for the PRU to finish '''                
+
+    ''' Catch events coming from the PRU '''                
     def _wait_for_events(self):
-        while self.running: 
-            if len(self.ddr_used) > 0:
-                print "About to wait for event"
-                pypruss.wait_for_event(PRU_EVTOUT_0)			# Wait a while for it to finish.
-                pypruss.clear_event(PRU0_ARM_INTERRUPT)			# Clear the event 
-                #print "double checking"
-                #pypruss.wait_for_event(PRU_EVTOUT_0)			# Wait a while for it to finish.
-                #pypruss.clear_event(PRU0_ARM_INTERRUPT)			# Clear the event 
-                ddr = self.ddr_used.pop(0)                      # Pop the first ddr memory amount         
-                print "pop "+str(ddr)+" remaining: "+str(len(self.ddr_used))
-            else:
-                time.sleep(1)
+        events_caught = 0
+        self.dev = os.open("/dev/uio0", os.O_RDONLY)
+        self.new_events = 0
+        self.old_events = 0
+        nr_interrupts = 0
+        while self.running:                    
+                self._wait_for_event()
+                pypruss.clear_event(PRU0_ARM_INTERRUPT)			# Clear the event        
+                nr_interrupts += 1
+                nr_events = struct.unpack("L", self.ddr_mem[self.DDR_END-4:self.DDR_END])[0]            
+                if nr_interrupts != nr_events:
+                    print "Error, nr of interrupts ("+str(nr_interrupt)+") != nr of events ("+str(nr_events)+")"
+                #print "Events: "+str(nr_events)+" nr of interrupts: "+str(nr_interrupts)
+                ddr = self.ddr_used.get()                       # Pop the first ddr memory amount           
+                with self.ddr_lock:
+                    self.ddr_mem_used -= ddr
+                    #print "Popped "+str(ddr)+"\tnow "+str(self.ddr_used.qsize())
+                self.ddr_used.task_done()
+
+    ''' Wait for an event. The resturn is the number of events that have occured since last check '''
+    def _wait_for_event(self):
+        self.new_events =  struct.unpack("L", os.read(self.dev, 4))[0]
+        ret = self.new_events-self.old_events
+        self.old_events = self.new_events
+        return ret
 
     ''' Close shit up '''
-    def close(self):
-        ddr_mem.close()                                         # Close the memory 
-        f.close()                                               # Close the file
+    def join(self):
+        logging.debug("joining")
+        self.running = False
+        self.t.join()        
+        self.ddr_mem.close()                                         # Close the memory        
         pypruss.pru_disable(0)                                  # Disable PRU 0, this is already done by the firmware
         pypruss.exit()                                          # Exit, don't know what this does. 
         
-
     ''' Convert delay in seconds to number of instructions for the PRU '''
     def _sec_to_inst(self, s):					    # Shit, I'm missing MGP for this??
         inst_pr_step  = s/self.s_pr_inst  		    # Calculate the number of instructions of delay pr step. 

software/Replicape.py

 import bbio as io
 from math import sqrt
 import time
+import Queue 
+import logging
 
 from Mosfet import Mosfet
 from Smd import SMD
 from Path import Path
 from Path_planner import Path_planner
     
+logging.basicConfig(level=logging.INFO)
+
 class Replicape:
     ''' Init '''
     def __init__(self):
         self.steppers["Z"]  = SMD(io.GPIO1_1,  io.GPIO2_2,  io.GPIO0_27, 2, "Z")  
         self.steppers["E"]  = SMD(io.GPIO3_21, io.GPIO3_19, io.GPIO2_3,  4, "Ext1")
 
-        # Enable the steppers and set current,  
+        # Enable the steppers and set the current, steps pr mm and microstepping  
         self.steppers["X"].setCurrentValue(2.0) # 2A
         self.steppers["X"].setEnabled() 
         self.steppers["X"].set_steps_pr_mm(6.105)         
         self.steppers["Z"].set_microstepping(2) 
 
         self.steppers["E"].setCurrentValue(1.8) # 2A        
-        #self.steppers["E"].setEnabled()
+        self.steppers["E"].setEnabled()
         self.steppers["E"].set_steps_pr_mm(5.0)
         self.steppers["E"].set_microstepping(2)
 
         self.ext1.setPvalue(0.5)
         self.ext1.setDvalue(0.1)     
         self.ext1.setIvalue(0.001)
-     
-        self.hbp = HBP( self.therm_hbp, self.mosfet_hbp)        # Make Heated Build platform 
-        #self.hbp.debugLevel(1)
+
+        # Make Heated Build platform 
+        self.hbp = HBP( self.therm_hbp, self.mosfet_hbp)       
 
         # Init the three fans
         self.fan_1 = Fan(1)
         self.fan_3 = Fan(3)
 
         # Make a queue of commands
-        self.queue = list()
+        self.commands = Queue.Queue()
 
         # Set up USB, this receives messages and pushes them on the queue
-        self.usb = USB(self.queue)		
+        self.usb = USB(self.commands)		
 
         # Get all options 
         self.options = Options()
 
-        # Make the positioning vector
-        self.position = {"x": 0, "y": 0, "z": 0}
-        
+        # Init the path planner
         self.movement = "RELATIVE"
         self.feed_rate = 3000.0
+        self.current_pos = {"X":0.0, "Y":0.0, "Z":0.0, "E":0.0}
+        self.acceleration = 300.0/1000.0
 
-        # Init the path planner
-        self.path_planner = Path_planner(self.steppers)         
-        self.path_planner.set_acceleration(300.0/1000.0) 
+        self.path_planner = Path_planner(self.steppers, self.current_pos)         
+        self.path_planner.set_acceleration(self.acceleration) 
+        logging.debug("Debug prints to console")
 	
     ''' When a new gcode comes in, excute it '''
     def loop(self):
         try:
-            while True:
-                if len(self.queue) > 0:
-                    gcode = Gcode(self.queue.pop(0), self)
-                    self._execute(gcode)
-                    self.usb.send_message(gcode.getAnswer())
-                else:
-                    io.delay(10)
+            while True:                
+                gcode = Gcode(self.commands.get(), self)
+                self._execute(gcode)
+                self.usb.send_message(gcode.getAnswer())
+                self.commands.task_done()
         except KeyboardInterrupt:
             print "Caught signal, exiting" 
             return
         finally:
-            self.cleanUp()  
+            self.ext1.disable()
+            self.hbp.disable()
+            self.usb.close() 
+            self.path_planner.exit()   
+        logging.debug("Done")
 		
     ''' Execute a G-code '''
     def _execute(self, g):
             for i in range(g.numTokens()):                          # Run through all tokens
                 axis = g.tokenLetter(i)                             # Get the axis, X, Y, Z or E
                 smds[axis] = float(g.tokenValue(i))                 # Get tha value, new position or vector             
-            path = Path(smds, feed_rate, self.movement)             # Make a path segment from the axes
-            while self.path_planner.nr_of_paths() > 10:              # If the queue is full, wait. 
-                time.sleep(1)                                       # This is the waiting part                
-            self.path_planner.add_path(path)                        # Ok, add the path to the planner queue
+            path = Path(smds, feed_rate, self.movement)             # Make a path segment from the axes            
+            self.path_planner.add_path(path)                        # Add the path. This blocks until the path planner has capacity
         elif g.code() == "G21":                                     # Set units to mm
             self.factor = 1.0
         elif g.code() == "G28":                                     # Home the steppers
             smds = {}                                               # All steppers 
             for i in range(g.numTokens()):                          # Run through all tokens
                 axis = g.tokenLetter(i)                             # Get the axis, X, Y, Z or E
-                smds[axis] = float(g.tokenValue(i))                # Get tha value, new position or vector             
-            print smds
-            path = Path(smds, self.feed_rate, "ABSOLUTE")             # Make a path segment from the axes
-            while self.path_planner.nr_of_paths() > 10:              # If the queue is full, wait. 
-                io.delay(100)                                       # This is the waiting part
-            self.path_planner.add_path(path)                        # Ok, add the path to the planner queue
+                smds[axis] = float(g.tokenValue(i))                 # Get tha value, new position or vector             
+            path = Path(smds, self.feed_rate, "ABSOLUTE")           # Make a path segment from the axes
+            self.path_planner.add_path(path)                        # Add the path. This blocks until the path planner has capacity
+        elif g.code() == "G29": 
+            print self.current_pos
         elif g.code() == "G90":                                     # Absolute positioning
             self.movement = "ABSOLUTE"
         elif g.code() == "G91":                                     # Relative positioning 
             self.hbp.setTargetTemperature(float(g.tokenValue(0)))
         else:
             print "Unknown command: "+g.message	
-
-    ''' Stop all threads '''
-    def cleanUp(self):
-        self.ext1.disable()
-        self.hbp.disable()
-        self.usb.close() 
-        self.path_planner.exit()   
    
 r = Replicape()
 r.loop()
     def setEnabled(self):
         if not self.enabled:
             self.state &= ~(1<<6)
-            self.update()
             self.enabled = True
-	
+            self.update()
+            	
     ''' Sets the SMD disabled '''
     def setDisabled(self):
         if self.enabled:
             self.state |= (1<<6)
-            self.update()
             self.enabled = False
+            self.update()           
 
     '''Logic high to enable device, logic low to enter
     low-power sleep mode. Internal pulldown.'''
 #from Gcode import Gcode
 from threading import Thread
 import select
+import logging
 
 class USB:
     def __init__(self, queue):
             ret = select.select( [self.tty],[],[], 1.0 )
     	    if ret[0] == [self.tty]:
                 message = self.tty.readline().strip("\n")          
-                if self.debug > 1:
-                    print "Message: "+message+" ("+message.encode("hex")+")"	
-                self.queue.append(message)
+                logging.debug("Message: "+message+" ("+message.encode("hex")+")")
+                self.queue.put(message)
             
 
     # Send a message		
     def send_message(self, message):
-        if self.debug > 1:           
-            print "USB: writing '"+message+"'"
+        logging.debug("USB: writing '"+message+"'")
         if message[-1] != "\n":
             message += "\n"
         self.tty.write(message)
     # Stop receiving mesassages
     def close(self):
         self.running = False
-        self.t.join(1.5)
+        self.t.join()