Commits

Elias Bakken committed c1d9a4e

Added the currently working kernel image for rev0, also faster merging of stepper data and the ddr wrap bug removed?

Comments (0)

Files changed (5)

images/3.2.34.zip

Binary file added.

images/uImage-3.2.34

Binary file added.
 '''
 
 import numpy as np                                                          # Needed for sqrt
+from numpy import linalg as la
 
 class Path: 	
     ''' The axes of evil, the feed rate in mm/min and ABS or REL '''
         self.movement = movement
         self.global_pos = {"X":0, "Y":0, "Z":0, "E":0} 
         self.actual_travel = axes.copy()
-        self.prev = None
-        self.next = None
-            
+           
     ''' Set the next path element '''
     def set_next(self, next):
         self.next = next
     ''' Set the previous path element '''
     def set_prev(self, prev):
         self.prev = prev
-        
+
     ''' Get the length of this path segment '''
     def get_length(self):      
         if "X" in self.axes:
         else:
             return self.axes[axis]/1000.0                         # If movement is relative, the vector is already ok. 
 
-    ''' Calculate the angle this path has with another path '''
-    def get_angle_with(self, next_path):
-        raise Exception("Not implemented!")
-
     ''' Get the top speed of this segment '''
     def get_max_speed(self):
         return (self.feed_rate/60.0)/1000.0
         return abs(self.get_axis_length(axis))/hyp
 
     ''' Get the lowest speed along this segment '''
-    def get_min_speed(self):
-        return 0
+    def get_end_speed(self):
+        return (1-self.angle_to_next()/np.pi)*self.get_max_speed()
 
+    ''' Get the lowest speed along this segment '''
+    def get_start_speed(self):
+        return (1-self.angle_to_prev()/np.pi)*self.get_max_speed()
 
     ''' Return the list of axes '''
     def get_axes(self):
     def set_global_pos(self, global_pos):
         self.global_pos = global_pos
 
+    ''' Return the angle to the next path segment '''
+    def angle_to_next(self):
+        if not hasattr(self, 'next'):
+            return 0
+        u = la.norm([self.get_axis_length("X"), self.get_axis_length("Y")])
+        v = la.norm([self.next.get_axis_length("X"), self.next.get_axis_length("Y")])
+        c = np.dot(u,v)/(u*v)
+        angle = np.arccos(c)
+
+        if np.isnan(angle):
+            if (u == v).all():
+                return 0.0
+            else:
+                return np.pi
+        return angle
+
+    ''' Return the angle to the previous path segment '''
+    def angle_to_prev(self):
+        if not hasattr(self, 'prev'):
+            return 0
+        u = la.norm([self.get_axis_length("X"), self.get_axis_length("Y")])
+        v = la.norm([self.prev.get_axis_length("X"), self.prev.get_axis_length("Y")])
+        c = np.dot(u,v)/(u*v)
+        angle = np.arccos(c)
+
+        if np.isnan(angle):
+            if (u == v).all():
+                return 0.0
+            else:
+                return np.pi
+        return angle
+        
+        
+if __name__ == '__main__':
+    a = Path({"X": 10, "Y": 0}, 3000, "RELATIVE")
+    b = Path({"X": -10, "Y": 0}, 3000, "RELATIVE")
+    c = Path({"X": 0, "Y": 10}, 3000, "RELATIVE")
+    
+    b.set_next(c)
+    b.set_prev(a)
+
+    print "Max speed "+str(b.get_max_speed())
+    print "Start speed "+str(b.get_start_speed())
+    print "End speed "+str(b.get_end_speed())
+
+
+
+
+
+
 

software/Path_planner.py

 from threading import Thread
 from Pru import Pru
 import Queue
+from collections import defaultdict
 
 class Path_planner:
     ''' Init the planner '''
     def __init__(self, steppers, current_pos):
         self.steppers    = steppers
         self.pru         = Pru()                                # Make the PRU
-        self.paths       = Queue.Queue(100)                      # Make a queue of paths
+        self.paths       = Queue.Queue(30)                      # 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.pru_data    = defaultdict(int)
         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.put(new)
+        if hasattr(self, 'prev'):
+            self.prev.set_next(new)
+            new.set_prev(self.prev)
+        self.prev = new        
         
     ''' Return the number of paths currently on queue '''
     def nr_of_paths(self):
                     data = self._make_data(path, axis)
                     if len(data[0]) > 0:
                         all_data[axis] = data                      # Generate the timing and pin data                         
-                        slowest = max(slowest, sum(data[1]))   
-                                
-
+                        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, 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
+                if "Z" in all_data:     # HACK! The Z-axis cannot be combined with the other data. Somehow it goes backwards...
                     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())
+                        #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() 
                     
                 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:
+                    z = zip(np.cumsum(packet[1]), packet[0])
+                    for item in z:
+                        self.pru_data[item[0]] += item[1]
+
+                if len(self.pru_data) > 0:
+                    z = zip(*sorted(self.pru_data.items()))
+                    self.pru_data = (list(z[1]), list(np.diff([0]+list(z[0]))))
+
+                    while not self.pru.has_capacity_for(len(self.pru_data[0])*8):
+                        time.sleep(0.1)                   
+                    self.pru.add_data(self.pru_data)
                     self.pru.commit_data()                            # Commit data to ddr
-                                     
+
+                self.pru_data = defaultdict(int)                    
                 self.paths.task_done()
                
             except Queue.Empty:
                 pass
+
+    def merge_data(self):
+        self.pru_data        
+
+    def _add_or_append(self, old, ts, pin):
+        if ts == old[-1][0]:
+            return (x, old[-1][1]+y) 
+        return (x, y)
+
+
     ''' Join the thread '''
     def exit(self):
         self.running = False
         pins        = [step_pin | dir_pin, dir_pin]*num_steps           # Make the pin states
 
         s           = abs(path.get_axis_length(axis))                   # Get the length of the vector
-        ratio       = path.get_axis_ratio(axis)
-
-        Vm = path.get_max_speed()*ratio				                    # The travelling speed in m/s
-        a  = self.acceleration*ratio    		                        # Accelleration in m/s/s
-        ds = 1.0/steps_pr_meter                                         # Delta S, distance in meters travelled pr step. 
-        u  = ratio*path.get_min_speed()                 	            # Minimum speed in m/s
-        tm = (Vm-u)/a					                                # Calculate the time for when max speed is met. 
-        sm = min(u*tm + 0.5*a*tm*tm, s/2.0)			                    # Calculate the distance traveled when max speed is met
-
-        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
+        ratio       = path.get_axis_ratio(axis)                         # Ratio is the length of this axis to the total length
+
+        Vm       = path.get_max_speed()*ratio				            # The travelling speed in m/s
+        a        = self.acceleration*ratio    		                    # Accelleration in m/s/s
+        ds       = 1.0/steps_pr_meter                                   # Delta S, distance in meters travelled pr step.         
+        if self.pru.is_processing():                                    # If there is currently a segment being processed, 
+            u_start  = ratio*path.get_start_speed()                 	    # The end speed, depends on the angle to the next
+        else:
+            u_start = 0
+        if self.paths.qsize() > 0:                                      # If there are paths in queue, we do not have to slow down
+            u_end    = ratio*path.get_end_speed()                 	    # The start speed. Depends on the angle to the prev.
+        else:
+            u_end = 0
+
+        #print "Max speed for "+axis+" is "+str(Vm)
+        #print "Start speed for "+axis+" is "+str(u_start)
+        #print "End speed for "+axis+" is "+str(u_end)
+        tm_start = (Vm-u_start)/a					                    # Calculate the time for when max speed is met. 
+        tm_end   = (Vm-u_end)/a					                        # Calculate the time for when max speed is met. 
+        sm_start = min(u_start*tm_start + 0.5*a*tm_start**2, s/2.0)     # Calculate the distance traveled when max speed is met
+        sm_end   = min(u_end*tm_end + 0.5*a*tm_end**2, s/2.0)           # Calculate the distance traveled when max speed is met
+
+        distances_start  = list(np.arange(0, sm_start, ds))		        # Table of distances                       
+        distances_end    = list(np.arange(0, sm_end, ds))		        # Table of distances                       
+        timestamps_start = [(-u_start+np.sqrt(2.0*a*ss+u_start**2))/a for ss in distances_start]# When ticks occur
+        timestamps_end   = [(-u_end  +np.sqrt(2.0*a*ss+u_end**2))/a for ss in distances_end]# When ticks occur
+        delays_start     = np.diff(timestamps_start)/2.0			         # We are more interested in the delays pr second. 
+        delays_end       = np.diff(timestamps_end)/2.0			         # We are more interested in the delays pr second.         
+        delays_start     = list(np.array([delays_start, delays_start]).transpose().flatten())         
+        delays_end       = list(np.array([delays_end, delays_end]).transpose().flatten()) 
+
+        i_steps     = 2*num_steps-len(delays_start)-len(delays_end)       # Find out how many delays are missing
+        i_delays    = [(ds/Vm)/2.0]*i_steps  		                    # Make the intermediate steps
+        delays      = delays_start+i_delays+delays_end[::-1]                  # Add the missing delays. These are max_speed
         td          = num_steps/steps_pr_meter                          # Calculate the actual travelled distance        
         if vec < 0:                                                     # If the vector is negative, negate it.      
             td     *= -1.0
         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)))
+        #with open(axis+"_delays", "w+") as f:
+        #    f.write(", ".join(map(str, delays)))
+
         return (pins, delays)                                           # return the pin states and the data
 
 
+if __name__ == '__main__':
+    import bbio as io
+    from Smd import SMD
+    from Path import Path
 
+    steppers = {}
+
+    current_pos = {"X": 0.0, "Y": 0.0}
+    # Init the 5 Stepper motors
+    steppers["X"]  = SMD(io.GPIO1_12, io.GPIO1_13, io.GPIO1_7,  7, "X")  # Fault_x should be PWM2A?
+    steppers["Y"]  = SMD(io.GPIO1_31, io.GPIO1_30, io.GPIO1_15, 1, "Y")  
+    path_planner = Path_planner(steppers, current_pos)         
+    path_planner.set_acceleration(0.3) 
+
+    path = Path({"X": 1000.0, "Y": 1000.0}, 3000.0, "RELATIVE")  
+    import profile
+    path_planner.add_path(path)
+    profile.run('path_planner.test()')
+    path_planner.add_path(path)
+    profile.run('path_planner.test2()')
+
+
+    #profile.run('loop_1()')
+    #profile.run('loop_2()')
+
+    print path_planner.pru._sec_to_inst(0.002)
+    print path_planner.pru._sec_to_inst_2(0.002)
+
+
+    #profile.run('path_planner._make_data(path, "X")')
+    path_planner.exit()
 
-'''
-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 time 
 import mmap
 import struct 
+import select
 
 DDR_MAGIC			= 0xbabe7175
 
     def __init__(self):
         pru_hz 			    = 200*1000*1000             # The PRU has a speed of 200 MHz
         self.s_pr_inst 		= 1.0/pru_hz                # I take it every instruction is a single cycle instruction
+        self.s_pr_inst_2    = 2.0*(1.0/pru_hz)          # I take it every instruction is a single cycle instruction
         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.sec_to_inst_dev = (self.s_pr_inst*2)
         self.pru_data       = []      	    	        # This holds all data for one move (x,y,z,e1,e2)
-        self.ddr_used       = Queue.Queue(100)           # List of data lengths currently in DDR for execution
+        self.ddr_used       = Queue.Queue(30)           # 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.debug = 0
+    
         self.i = 0
         pypruss.modprobe(0x40000)    			        # This only has to be called once pr boot
         self.ddr_addr = pypruss.ddr_addr()
         delays = map(self._sec_to_inst, delays)     	    # Convert the delays in secs to delays in instructions
         data = np.array([pins, delays])		        	    # Make a 2D matrix combining the ticks and delays
         data = list(data.transpose().flatten())     	    # Braid the data so every other item is a pin and delay
-        
-        self.ddr_reserved += len(data)*4		            # This amount is now reserved, waiting to be committed       
-
         if len(self.pru_data) > 0:
             self.pru_data = self._braid_data(data, self.pru_data)
         else:
             self.pru_data = data
-        return 1
+        return 1    
 
     ''' Check if the PRU has capacity for a chunk of data '''
     def has_capacity_for(self, data_len):
             cap = self.ddr_size-self.ddr_mem_used
         return cap
 
+    ''' Returns True if there are segments on queue '''
+    def is_processing(self):
+        return (self.ddr_used.qsize() > 0)
+
     ''' Commit the data to the DDR memory '''
-    def commit_data(self):
+    def commit_data(self, two=False):
         data = struct.pack('L', len(self.pru_data)/2)	    	# Data in string form
-        for reg in self.pru_data:									
-            data += struct.pack('L', reg) 				        # Make the data, it needs to be a string
+        data += ''.join([struct.pack('L', word) for word in self.pru_data])
         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-16:                        # 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
+            print "self.ddr_end >= self.DDR_END-16"
             multiple = (self.DDR_END-self.ddr_start)%8          # Find a multiple of 8
             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
+            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.
 
             with self.ddr_lock:
                 self.ddr_mem_used += len(first)
             self.ddr_used.put(len(first))
-            self.ddr_reserved -= len(first)
 
             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.
+                with self.ddr_lock:
+                    self.ddr_mem_used += len(second)
+                self.ddr_used.put(len(second))
+
             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"
+                self.ddr_end = self.DDR_START+4
+                self.ddr_mem[self.DDR_START:self.DDR_START+4] = struct.pack('L', 0) # Terminate the first word
+                self.debug = 2
+                print "\tSecond batch skipped, 0 length"
+                print "\tremaining data "+str(data[cut:])
             
             print "Wrapped. Capacity is now "+str(self.get_capacity())
         else:
+            if self.debug > 0:
+                 print "Laying out from "+hex(self.ddr_start)+" to "+hex(self.ddr_end)
+                 print "self.ddr_end = "+hex(self.ddr_end)
+                 print "self.DDR_END = "+hex(self.DDR_END)
             self.ddr_mem[self.ddr_start:self.ddr_end] = data    # Write the data to the DDR memory.
+            with self.ddr_lock:
+                self.ddr_mem_used += len(data)
+            self.ddr_used.put(len(data)) 		            # update the amount of memory used 
 
         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         
 
 
         self.new_events = 0
         self.old_events = 0
         nr_interrupts = 0
-        while self.running:                    
+        while self.running:
+            ret = select.select( [self.dev],[],[], 1.0 )
+            if ret[0] == [self.dev]:
                 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()
+                while nr_interrupts < nr_events:
+                    ddr = self.ddr_used.get()                       # Pop the first ddr memory amount           
+                    with self.ddr_lock:
+                        self.ddr_mem_used -= ddr                    
+                    if self.debug > 0:
+                        print "Popped "+str(ddr)+"\tnow "+hex(self.get_capacity())
+                    self.ddr_used.task_done()
+                    nr_interrupts += 1                         
 
     ''' Wait for an event. The resturn is the number of events that have occured since last check '''
     def _wait_for_event(self):
         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??
+    def _sec_to_inst_2(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. 
         inst_pr_step /= 2.0					        # To get a full period, we must divide by two. 
         inst_pr_step -= self.inst_pr_loop		    # Remove the "must include" number of steps
         inst_pr_step /= self.inst_pr_delay		    # Yes, this must be right..
+        #inst_pr_step = ((s/self.s_pr_inst)-self.inst_pr_loop)/self.inst_pr_delay
         if inst_pr_step < 1:
             inst_pr_step = 1
         return int(inst_pr_step)			        # Make it an int
 
+    ''' 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 = (int(s/self.s_pr_inst_2)-self.inst_pr_loop)/self.inst_pr_delay
+        if inst_pr_step < 1:
+            inst_pr_step = 1
+        return inst_pr_step
 
     ''' Braid/merge together the data from the two data sets'''
     def _braid_data(self, data1, data2):
         return braids
 
 
+
+'''
+        if two:
+            data = struct.pack('L', len(self.pru_data)/2)	    	# Data in string form
+            #print list(sum(self.pru_data, ()))
+            self.pru_data = [(self._sec_to_inst(x), y) for x, y in self.pru_data]
+            data += ''.join([struct.pack('L', word) for word in list(sum(self.pru_data, ()))])
+            data += struct.pack('L', 0)                     # Add a terminating 0, this keeps the fw waiting for a new command.
+
+        else:
+ '''