URL
https://opencores.org/ocsvn/ssbcc/ssbcc/trunk
Subversion Repositories ssbcc
[/] [ssbcc/] [trunk/] [core/] [9x8/] [peripherals/] [stepper_motor.py] - Rev 12
Go to most recent revision | Compare with Previous | Blame | View Log
################################################################################ # # Copyright 2015, Sinclair R.F., Inc. # ################################################################################ import math from ssbccPeripheral import SSBCCperipheral from ssbccUtil import CeilLog2 from ssbccUtil import SSBCCException class stepper_motor(SSBCCperipheral): """ Stepper motor driver\n This peripheral creates pulses to driver a stepper motor driver such as TI's DRV8825. It includes a buffer which can be used to store acceleration, motion, and deceleration profiles and returns a "completed" status to the micro controller.\n The core runs accumulators for the angle and the rate. I.e. the rate is an accumulated sum of the initial rate and the commanded acceleration and the angle is an accumulation of this possibly changing rate. The direction signal (if present) to the stepper motor is the sign bit from the accumulated rate. The "step" signal to the stepper motor driver is strobed every time the accumulated angle overflows or underflows and the direction bit to the driver is set according to whether the accumulated angle overflowed or underflowed.\n The motor control word consists of the following signals: initial rate acceleration number of steps to be performed this control word optional mode specification These control words are individually packed into one or more 8-bit bytes. The 8-bit values from the micro controller are shifted into a buffer from which the control word is constructed. This completed control word is then shifted into a FIFO. The control words in the FIFO are then used to generate the timing for the step strobes to the stepper motor driver. When the FIFO empties, the rate and the acceleration are set to zero and the mode retains its most recent value. The user must ensure they do not overfill the FIFO -- the only FIFO status is an "empty" or control words "done" status.\n Usage: PERIPHERAL stepper_motor basename=name \\ outcontrol=O_name \\ outrecord=O_name \\ outrun=O_name \\ indone=I_name \\ [inerror=I_name] \\ [nodir] \\ ratemethod={CLK_FREQ_HZ/RATE_HZ|count} \\ ratescale=N_rate_scale \\ rateres=N_rate \\ accelscale=N_accel_scale \\ accelres=N_accel \\ [accumres=N_accum] \\ countwidth=N_count \\ [modewidth=N_mode] \\ [FIFO=N_fifo]\n Or:\n PERIPHERAL stepper_motor basename=name \\ master=mastername \\ outrecord=O_name \\ outrun=O_name \\ indone=I_name \\ [nodir] \\ [inerror=I_name] \\ [FIFO=N_fifo]\n Where: basename=name specifies the name used to contruct the I/O signals Note: The name must start with an alphabetic character. Example: "basename=stepper" results in the names "o_stepper_dir", "o_stepper_step", and "o_stepper_mode" for the output direction, step, and optional mode signals and "i_stepper_error" for the input error signal. master=mastername specifies a preceding stepper_motor peripheral to use for the internal clock and to use for the accleration, rate, angle accumulator, and mode sizes Note: The "outcontrol" port from the master peripheral is used to queue the control words for its slaves. outcontrol=O_name specifies the port used to assemble 8-bit control values into the stepper motor control word Note: The name must start with "O_". outrecord=O_name specifies the port used to generate the strobe that pushes the assembled motor control word into the stepper motor FIFO Note: The name must start with "O_". outrun=O_name specified the port used to begin the sequence of operations specified by the motor control words in the buffer Note: The name must start with "O_". indone=I_name specifies the port used to determine whether or not the operations in the FIFO have finished Note: The name must start with "I_". inerror=I_name optionally specifies the port used to read the error status from the stepper motor controller Note: The name must start with "I_". nodir optionally specify that the stepper motor does not generate a direction bit ratemethod specified the method to generate the internal clock rate from the processor clock 1st method: CLK_FREQ_HZ/RATE_HZ CLK_FREQ_HZ is the frequency of "i_clk" in Hz a number will be interpreted as the clock frequency in Hz a symbol will be interpreted as a constant or a parameter Note: the symbol must be declared with the CONSTANT, LOCALPARARM, or PARAMETER configuration command. RATE_HZ is the desired internal clock rate this is specified as per "CLK_FREQ_HZ" 2nd method: count specify the number of "i_clk" clock cycles per internal clock cycle Note: CLK_FREQ_HZ, RATE_HZ, and count can be parameters or constants. For example, the following uses the parameter C_CLK_FREQ_HZ for the clock frequency and an internal rate to 500 kHz: "ratemethod=C_CLK_FREQ_HZ/500_000". Note: The minimum value of "ratemethod" is 2. ratescale=N_rate_scale specifies the scaling for the most significant bit of the rate Note: See the 'a' parameter in the "Theory of Operation" section. rateres=N_rate specifies the resolution of the rate Note: See the 'r' parameter in the "Theory of Operation" section. accelscale=N_accel_scale specifies the scaling for the most significant bit of the acceleration Note: See the 'a' parameter in the "Theory of Operation" section. accelres=N_accel specifies the resolution for the acceleration Note: See the 'b' parameter in the "Theory of Operation" section. accumres=N_accum optionally specify the resolution for the accumulator to the summed angle (from which the step strobes are generated) Note: This must be between rateres and accelres. Note: The default value is accelres. countwidth=N_count specifies the width of the counter for the number of steps to be performed by the control word modewidth=N_mode - if not specified, there is no mode signal to the stepper motor controller - if specified then this specifies the width of the signal to the stepper motor controller FIFO=N_fifo optionally specify the depth of the control word FIFO Note: This must be a power of 2 and must be at least 16. Note: The default is 16.\n Theory of Operation: Define the following: n is the number of internal clock cycles since the control word started being performed (i.e., these are counted after the dlock rate is reduced by ratemethod) F is the internal clock cycle frequency (i.e., RATE_HZ in the second form for specifying the ratemethod R_0 is the initial rate command R_n is the accumulated rate after n internal clock cycles A is the commanded acceleration S_n is the accumulated step after n internal clock cycles Then R_n = R_0 + A * n S_n = R_0 * n + A * n * (n-1) / 2 The rate R_n can be thought of as a signed fraction with the format "s0.r" where 's' represents the sign bit, there are no bits to the left of the decimal, and there are 'r' bits to the right of the decimal. Then the rate can be as high as F and as low as F/2^r. Practically, the maximum rate cannot exceed half the internal clock frequency, otherwise the "step" signals will merge together and the stepper driver will not see distinct driver pulses.\n Similarly, the acceleration command A can be thought of as a signed fraction with the format "sa.b". Here 's' again represents the sign bit and 'b' represents the number of bits to the right of the decimial, but 'a' is a negative number representing the first bit in A. I.e., aside from the sign bit, A is b+a+1 bits wide. For example, the specification s-4.8 means that A has a sign bit with 8-4+1 = 5 bits for the value of A with the leasts significant bit representing a rate of F^2/2^8.\n The bit widths are determined as follows: Let mR be the minimum non-zero magnitude of the rate, mA be the minimum non-zero mangitude of the acceleration, and MA be the mamximum magnitude of the acceleration, all in step/sec or step/sec^2. Then\n r = ceil(-log_2(mR/F))\n a = floor(log_2(MA/F^2))\n b = ceil(-log_2(mA/f^2))\n Note: r and b may be increased by a few bits if accurate representations of the minimum rates are needed.\n Example: A micro controller with an 8 MHz clock is used to operate a DRV8825 driving a stepper motor assembly. The stepper motor has 200 steps per revolution, can be operated in full-step or a 16-step micro step mode, has a maximum rotation rate of 10 Hz, and has a maximum acceleration of 4 Hz/sec (i.e., 800 full-steps/sec^2). The motor is attached to a 400mm theaded rod with a pitch of 4mm per revolution.\n The 1.9usec minimum high and low widths of the DRV8825 and the 8 MHz processor clock mean that the stepper motor controller can realistically be run at 500kHz. The rate method to divide the micro controller clock to the internal processing rate is specified by "ratemethod" in the PERIPHERAL command.\n The bit widths are determine by choosing:\n MR = 10 rev/sec * 200 full-step/rev * 16 micro-step/full-step = 32000 micro-step/sec ==> R = -ceil(log_2(MR/F)) = -ceil(log_2((32000 micro-step/sec)/500kHz)) = 3 mR = 10 step/sec ==> r = -floor(log_2(mR/F)) = -floor(log_2((10 step/sec)/500kHz) = 16\n MA = 10 rev/sec^2 = 10*16*200 step/sec^2 ==> A = -ceil(log_2(MA/F^2)) = -ceil(log_2((32,000 step/sec^2)/500kHz^2)) = 22\n mA = 20 step/sec^2 (in full step mode) ==> a = -floor(log_2(mA/F^2)) = 34\n The values R=3 and r=16 along with the sign bit mean the rate would be stored in a signed 14-bit value. The rate requires two 8-bit writes to the control word.\n The values A=22 and a=34 mean the acceleration would be stored in a signed 1+(34-22) = 13 bit value. The acceleration requires two 8-bit writes to the control word.\n The accumulator width is set to the same value as the acceleration resolution. This avoid non-linear trunction errors and makes the motion profile more predictable using simple integer arithmetic.\n The number of full steps to move from one of the of rod to the other is (400mm/(4mm/rev)*(200steps/rev)=20_000 steps. In the micro-stepmode there are 16 micro steps per full step, so at most 320_000 micro steps can be performed before the full length of the rod is traversed. I.e., a 19-bit counter will suffice for the worst-case unidirection motion. This 19-bit count requires 3 8-bit writes to the control word.\n A "modewidth" of 1 is specifies so that the controller can be operated in either full step or a single hard-wired micro-step mode. If all 3 of the DRV8825 mode pins were connected, then "modewidth=3" would need to specified.\n The peripheral is then specified as follows:\n CONSTANT C_RATE_SCALE 3 CONSTANT C_RATE_RES 16 CONSTANT C_ACCEL_SCALE 22 CONSTANT C_ACCEL_RES 34 CONSTANT C_ACCUM_RES 34 CONSTANT C_COUNT_WIDTH 19 PERIPHERAL stepper_motor basename=stepper \\ outcontrol=O_stepper_control \\ outrecord=O_stepper_wr \\ outrun=O_stepper_go \\ indone=I_stepper_done \\ inerror=I_stepper_error \\ ratemethod=8_000_000/500_000 \\ ratescale=C_RATE_SCALE \\ rateres=C_RATE_RES \\ accelscale=C_ACCEL_SCALE \\ accelres=C_ACCEL_RES \\ accumres=C_ACCUM_RES \\ countwidth=C_COUNT_WIDTH \\ modewidth=1\n and the TBD byte control words are pushed into the peripheral as follows: R_0 14-bit initial rate stored in a 16-bit field (MSB first) A 13-bit acceleration stored in a 16-bit field (MSB first) COUNT 19-bit count stored in a 24-bit field (MSB first) MODE 1-bit mode stored as the lsb of an 8-bit field The control word is a total of 8 bytes wide.\n To command the peripheral to accelerate from stop to 200 steps/sec in one second in the forward direction using the full-step mode, the following seqeuence of bytes would be written to the control port: 0x00 0x00 ; initial rate is zero 0x00 0x0E ; 200 step/sec^2 * 2^34 / 500kHz^2 = 14 0x00 0x00 0x63 ; send 100 step commands (command 100-1=99) 0x00 ; full-step mode Note: It will take t=sqrt(2*100*2^34/14)/F = 0.99 sec to move the commanded 100 steps. At this time the speed will be r=t*14/2^34*F^2 = 201 step/sec. A more accurate match to the commanded speed could be accomplished by adding additional bits to the acceleration resolution at the cost of using more FPGA resources. Alternatively, the acceleration could be commanded for 99 steps and any subsequent 200 step/sec motion could be lengthened by 1 step. Another alternative would be to use a micro-step acceleration. Practically, the computed command is within 0.5% of the desired step rate.\n To command the peripheral to decelerate from 200 step/sec to zero in one second, the following sequence of bytes would be written to the control port: 0x00 0x01 0xDB ; 200 step/sec * 2^23 / 500kHz 0xFF 0x9C ; negative of the above acceleration 0x00 0x00 0x63 ; send 100 step commands (command 100-1=99) 0x00 ; full-step mode\n The first of these two control words could be assembled and transmitted to the peripheral as follows:\n 0x00 ; mode .push24(${100-1}) ; send 100 step commands .push16(14) ; 200 step/sec^2 .push16(0) ; initial rate is zero ${8-1} :loop swap .outport(O_stepper_control) .jumpc(loop,1-) drop .outstrobe(O_stepper_wr) ; push the assembed control word into the FIFO ... .outstrobe(O_stepper_go) ; perform the queued control words\n Example: Slave a second stepper motor controller peripheral to the preceding periperal.\n PERIPHERAL stepper_motor basename=slave \\ master=stepper \\ outrecord=O_slave_wr \\ outrun=O_slave_go \\ indone=I_slave_done \\ inerror=I_slave_error\n This controller will use the internal clock generated by the first controller and the scales, resolutions, and accumulator width will be the same as that master peripheral. What will be different is the four I/O ports used to operate and status the controller. """ def __init__(self,peripheralFile,config,param_list,loc): # Use the externally provided file name for the peripheral self.peripheralFile = peripheralFile # Get the parameters. allowables = ( ( 'FIFO', r'\S+$', lambda v : self.IntPow2Method(config,v,lowLimit=16), ), ( 'accelres', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ( 'accelscale', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ( 'accumres', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ( 'basename', r'[A-Za-z]\w*$', None, ), ( 'countwidth', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ( 'indone', r'I_\w+$', None, ), ( 'inerror', r'I_\w+$', None, ), ( 'master', r'[A-Za-z]\w*$', None, ), ( 'modewidth', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ( 'nodir', None, None, ), ( 'outcontrol', r'O_\w+$', None, ), ( 'outrecord', r'O_\w+$', None, ), ( 'outrun', r'O_\w+$', None, ), ( 'ratemethod', r'\S+$', lambda v : self.RateMethod(config,v), ), ( 'rateres', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ( 'ratescale', r'\S+$', lambda v : self.IntMethod(config,v,lowLimit=1), ), ) names = [a[0] for a in allowables] for param_tuple in param_list: param = param_tuple[0] if param not in names: raise SSBCCException('Unrecognized parameter "%s" at %s' % (param,loc,)) param_test = allowables[names.index(param)] self.AddAttr(config,param,param_tuple[1],param_test[1],loc,param_test[2]) # Signals that can't be specified when a master is specified. masterExclude = ( 'accelres', 'accelscale', 'accumres', 'countwidth', 'modewidth', 'outcontrol', 'ratemethod', 'rateres', 'ratescale', ) # Ensure the required parameters are provided. reqdParms = ( 'basename', 'indone', 'outrecord', 'outrun', ) if not hasattr(self,'master'): reqdParms += tuple([me for me in masterExclude if me not in ('accumres','modewidth',)]) for paramname in reqdParms: if not hasattr(self,paramname): raise SSBCCException('Required parameter "%s" is missing at %s' % (paramname,loc,)) # Ensure mutually exclusive parameters are not listed. if hasattr(self,'master'): for paramname in masterExclude: if hasattr(self,paramname): raise SSBCCException('Parameter "%s" cannot be specified alongside "master" at %s' % (paramname,loc,)) # Ensure basename is unique for this class of peripheral for p in config.peripheral: if (str(p.__class__) == str(self.__class__)) and (p.basename == self.basename): raise SSBCCException('Duplicated stepper_motor basename "%s" at %s' % (self.basename,loc,)) # For slaves, copy the bit widths from the master peripheral. if hasattr(self,'master'): for p in config.peripheral: if (str(p.__class__) == str(self.__class__)) and (p.basename == self.master): break else: raise SSBCCException('Can\'t find preceding stepper_motor peripheral with basename=%s at %s ' % (self.master,loc,)) self.master = p for paramname in masterExclude: setattr(self,paramname,getattr(self.master,paramname)) # Set unspecified optional parameters. if not hasattr(self,'accumres'): self.accumres = self.accelres if not hasattr(self,'modewidth'): self.modewidth = 0 if not hasattr(self,'FIFO'): self.FIFO = 16 # Ensure the parameters satisfy any mutual constraints. if not (self.rateres < self.accelres): raise SSBCCException('rateres should be smaller than accelres at %s' % loc) if not (self.rateres <= self.accumres <= self.accelres): raise SSBCCException('accumres must be between rateres and accelres at %s' % loc) # Add the I/O port, internal signals, and the INPORT and OUTPORT symbols for this peripheral. if not hasattr(self,'nodir'): config.AddIO('o_%s_dir' % self.basename, 1, 'output', loc) config.AddIO('o_%s_step' % self.basename, 1, 'output', loc) if self.modewidth > 0: config.AddIO('o_%s_mode' % self.basename, 1, 'output', loc) if hasattr(self,'inerror'): config.AddIO('i_%s_error' % self.basename, 1, 'input', loc) config.AddSignal('s__%s__done' % self.basename, 1, loc) self.ix_outcontrol = config.NOutports() if not hasattr(self,'master'): config.AddOutport((self.outcontrol, False, # empty list ),loc) self.ix_outrecord = config.NOutports() config.AddOutport((self.outrecord, True, # empty list ),loc) self.ix_outrun = config.NOutports() config.AddOutport((self.outrun, True, # empty list ),loc) if hasattr(self,'inerror'): config.AddInport((self.inerror, ('i_%s_error' % self.basename, 1, 'data', ), ), loc) config.AddInport((self.indone, ('s__%s__done' % self.basename, 1, 'data', ), ), loc) # Compute bit widths. dw = config.Get('data_width') self.data_width = dw self.ratecmdwidth = 1 + self.rateres - self.ratescale self.ratewidth = 1 + self.accelres - self.ratescale self.accelwidth = 1 + self.accelres - self.accelscale self.accumwidth = self.accumres + 1 self.controlwidth = self.ratecmdwidth self.controlwidth += dw*int((self.accelwidth+dw-1)/dw) self.controlwidth += dw*int((self.countwidth+dw-1)/dw) self.controlwidth += dw*int((self.modewidth+dw-1)/dw) self.controlwidthpacked = self.ratecmdwidth + self.accelwidth + self.countwidth + self.modewidth # Add the 'clog2' function to the processor (if required). config.functions['clog2'] = True def GenVerilog(self,fp,config): body = self.LoadCore(self.peripheralFile,'.v') if hasattr(self,'master'): body = re.sub(r'@MASTER_BEGIN@.*?@MASTER_END@\n','',body,flags=re.DOTALL) else: body = re.sub(r'@MASTER_BEGIN@\n','',body) body = re.sub(r'@MASTER_END@\n','',body) if self.modewidth == 0: body = re.sub(r'@OUTMODE_BEGIN@.*?@OUTMODE_END@\n','',body,flags=re.DOTALL) else: body = re.sub(r'@OUTMODE_BEGIN@\n','',body) body = re.sub(r'@OUTMODE_END@\n','',body) if hasattr(self,'nodir'): body = re.sub(r' *o__dir.*?\n','',body) masterBasename = self.basename if not hasattr(self,'master') else self.master.basename for subpair in ( ( r'@ACCEL_WIDTH@', str(self.accelwidth), ), ( r'@ACCEL_RES@', str(self.accelres), ), ( r'@ACCEL_SCALE@', str(self.accelscale), ), ( r'@ACCUM_RES@', str(self.accumres), ), ( r'@ACCUM_WIDTH@', str(self.accumwidth), ), ( r'@CONTROL_WIDTH@', str(self.controlwidth), ), ( r'@CONTROL_WIDTH_PACKED@', str(self.controlwidthpacked), ), ( r'@COUNT_WIDTH@', str(self.countwidth), ), ( r'@DW@', str(self.data_width), ), ( r'@DWM1@', str(self.data_width-1), ), ( r'@FIFO_DEPTH@', str(self.FIFO), ), ( r'@IX_OUTCONTROL@', str(self.ix_outcontrol), ), ( r'@IX_OUTRECORD@', str(self.ix_outrecord), ), ( r'@IX_OUTRUN@', str(self.ix_outrun), ), ( r'@MODE_WIDTH@', str(self.modewidth), ), ( r'@NAME@', self.basename, ), ( r'@NBITS_FIFO_DEPTH@', str(CeilLog2(self.FIFO)), ), ( r'@OUTMODEWIDTH@', str(self.modewidth), ), ( r'@RATECMD_WIDTH@', str(self.ratecmdwidth), ), ( r'@RATEMETHOD@', str(self.ratemethod), ), ( r'@RATE_RES@', str(self.rateres), ), ( r'@RATE_SCALE@', str(self.ratescale), ), ( r'@RATE_WIDTH@', str(self.ratewidth), ), ( r'\bL__', 'L__%s__' % self.basename, ), ( r'\bi__', 'i_%s_' % self.basename, ), ( r'\bo__', 'o_%s_' % self.basename, ), ( r'\bs__', 's__%s__' % self.basename, ), ( r'@S__CLK_EN@', 's__%s__clk_en' % masterBasename, ), ( r'@S__INPUT_CONTROL_WORD_PACKED@', 's__%s__input_control_word_packed' % masterBasename, ), ): body = re.sub(subpair[0],subpair[1],body) body = self.GenVerilogFinal(config,body) fp.write(body)
Go to most recent revision | Compare with Previous | Blame | View Log