OpenCores
URL https://opencores.org/ocsvn/ssbcc/ssbcc/trunk

Subversion Repositories ssbcc

[/] [ssbcc/] [trunk/] [core/] [9x8/] [peripherals/] [servo_motor.py] - Blame information for rev 9

Details | Compare with Previous | View Log

Line No. Rev Author Line
1 9 sinclairrf
################################################################################
2
#
3
# Copyright 2015, Sinclair R.F., Inc.
4
#
5
################################################################################
6
 
7
import math
8
 
9
from ssbccPeripheral import SSBCCperipheral
10
from ssbccUtil import CeilLog2
11
from ssbccUtil import SSBCCException
12
 
13
class servo_motor(SSBCCperipheral):
14
  """
15
  Servo Motor driver:\n
16
  Creates PWM modulated signals to operate micro servos for UAVs and similar.\n
17
  Usage:
18
    PERIPHERAL servo_motor      outport=O_name                          \\
19
                                {outsignal|outsignaln}=o_name           \\
20
                                freq_hz=FREQ_HZ                         \\
21
                                min_width=XXX{s|ms|us|ns}               \\
22
                                max_width=XXX{s|ms|us|ns}               \\
23
                                [default_width=XXX{s|ms|us|ns}]         \\
24
                                {period=XXX{s|ms|us|ns}|sync=o_name}    \\
25
                                [inperiod=I_name]                       \\
26
                                [scale=C_name]                          \\
27
                                [scale_max=C_name]\n
28
  Where:
29
    outport=O_name
30
      specifies the symbol used to write the 8-bit PWM control to this
31
      peripheral
32
      Note:  The name must start with "O_".
33
    {outsignal|outsignaln}=o_name
34
      specifies the name of the output PWM signal
35
      Note:  outsignal creates a positive pulse for the PWM control and
36
             outsignaln creates an inverted pulse for the PWM control.
37
      Note:  The name must start with "o_".
38
    freq_hz=FREQ_HZ
39
      specifies the processor clock speed to the peripheral
40
    min_width=XXX{s|ms|us|ns}
41
      specifies the minimum pulse width
42
      Note:  XXX may be an integer or a real number
43
      Note:  The minimum width must be a realizable positive value (since a
44
             pulse width of zero means no control is being given to the servo).
45
    max_width=XXX{s|ms|us|ns}
46
      specifies the maximum pulse width
47
      Note:  XXX may be an integer or a real number
48
    default_width=XXX{s|ms|us|ns}
49
      optionally specifies the default width of the PWM before it is set by the
50
      processor
51
      Note:  If the default width is not specified then the minimum width is
52
             used as the default width.
53
    {period=XXX{s|ms|us|ns}|sync=o_name}
54
      either specifies the rate at which the PWM is generated or synchronize the
55
      PWM generation to a preceding servo_motor peripheral with the output
56
      signal o_name
57
      Note:  XXX may be an integer or a real number
58
      Note:  When sync is specified the leading edges of the PWMs will coincide.
59
    inperiod=I_name
60
      optionally specifies an input port to receive the strobe generated by the
61
      associated period
62
      Note:  This optional parameter requires that period be specified.  It is
63
             not compatible with the sync parameter.
64
    scale=C_name
65
      optionally creates a constant named "C_name" which states how many clock
66
      cycles are used for each count in the 8-bit PWM control
67
      Note:  The name must start with "C_".
68
      Example:  A PWM range of 1000 to 1500 usec and a clock frequency of 8 MHz
69
                produces a value of ceil((1500-1000)*8/(256-1)) = 16 clock
70
                cycles per 8-bit control value count.
71
    scale_max=C_name
72
      optionally creates a constant named "C_name" wich states the upper limit
73
      of the continuous control range
74
      Note:  The name must start with "C_".
75
      Example:  For the example in "scale=C_name" the upper limit would be
76
                ceil((1500-1000)*8/16) = 250.  I.e., control values of 251
77
                through 255 inclusive will produce the same PWM width as the
78
                control value 250.
79
  Example:
80
    A micro servo that responds to positive PWM pulses between 1000 usec and
81
    1500 usec once every 20 msec and the processor clock is 8 MHz.  The
82
    architecture file would include the following:\n
83
      CONSTANT          C_FREQ_HZ       8_000_000
84
      PORTCOMMENT       servo motor control
85
      PERIPHERAL        servo_motor    outport=O_SERVO                  \\
86
                                       outsignal=o_servo                \\
87
                                       freq_hz=C_FREQ_HZ                \\
88
                                       min_width=1000us                 \\
89
                                       max_width=1500us                 \\
90
                                       period=20ms                      \\
91
                                       scale=C_servo_scale              \\
92
                                       scale_max=C_servo_scale_max\n
93
    will create a peripheral generating the desired PWM signal.\n
94
    The constants C_servo_scale and C_servo_scale_max could be reported by the
95
    micro controller to a controlling application to specify the sensitivity and
96
    upper limit for the servo motor controller.\n
97
  Example:
98
    Synchronize a second servo motor with PWM pulses between 1000 usec and 2500
99
    usec to the preceding servo motor controller:\n
100
      PERIPHERAL        servo_motor     outport=O_SERVO_2               \\
101
                                        outsignal=o_servo2              \\
102
                                        freq_hz=C_FREQ_HZ               \\
103
                                        min_width=1.0ms                 \\
104
                                        max_width=2.5ms                 \\
105
                                        sync=o_servo\n
106
  """
107
 
108
  def __init__(self,peripheralFile,config,param_list,loc):
109
    # Use the externally provided file name for the peripheral
110
    self.peripheralFile = peripheralFile;
111
    # Get the parameters.
112
    allowables = (
113
      ( 'default_width',        r'\S+',         lambda v : self.TimeMethod(config,v), ),
114
      ( 'freq_hz',              r'\S+$',        lambda v : self.IntMethod(config,v), ),
115
      ( 'inperiod',             r'I_\w+$',      None,   ),
116
      ( 'max_width',            r'\S+$',        lambda v : self.TimeMethod(config,v), ),
117
      ( 'min_width',            r'\S+$',        lambda v : self.TimeMethod(config,v), ),
118
      ( 'outport',              r'O_\w+$',      None,   ),
119
      ( 'outsignal',            r'o_\w+$',      None,   ),
120
      ( 'outsignaln',           r'o_\w+$',      None,   ),
121
      ( 'period',               r'\S+$',        lambda v : self.TimeMethod(config,v), ),
122
      ( 'scale',                r'C_\w+$',      None,   ),
123
      ( 'scale_max',            r'C_\w+$',      None,   ),
124
      ( 'sync',                 r'o_\w+$',      None,   ),
125
    )
126
    names = [a[0] for a in allowables];
127
    for param_tuple in param_list:
128
      param = param_tuple[0];
129
      if param not in names:
130
        raise SSBCCException('Unrecognized parameter "%s" at %s' % (param,loc,));
131
      param_test = allowables[names.index(param)];
132
      self.AddAttr(config,param,param_tuple[1],param_test[1],loc,param_test[2]);
133
    # Ensure the required parameters are provided.
134
    for paramname in (
135
      'outport',
136
      'freq_hz',
137
      'min_width',
138
      'max_width',
139
    ):
140
      if not hasattr(self,paramname):
141
        raise SSBCCException('Required parameter "%s" is missing at %s' % (paramname,loc,));
142
    # Ensure exactly one of mandatory exclusive pairs are specified.
143
    for exclusivepair in (
144
      ( 'outsignal',    'outsignaln',   ),
145
      ( 'period',       'sync',         ),
146
    ):
147
      if not hasattr(self,exclusivepair[0]) and not hasattr(self,exclusivepair[1]):
148
        raise SSBCCException('One of %s or %s must be specified at %s', (exclusivepair[0], exclusivepair[1], loc, ));
149
      if hasattr(self,exclusivepair[0]) and hasattr(self,exclusivepair[1]):
150
        raise SSBCCException('Only one of %s or %s may be specified at %s', (exclusivepair[0], exclusivepair[1], loc, ));
151
    # Set optional signals
152
    if not hasattr(self,'default_width'):
153
      self.default_width = self.min_width;
154
    # Ensure signal values are reasonable.
155
    if self.min_width >= self.max_width:
156
      raise SSBCCException('min_width must be smaller than max_width at %s' % loc);
157
    if not self.min_width <= self.default_width <= self.max_width:
158
      raise SSBCCException('default_width is not between min_width and max_width at %s' % loc);
159
    # Ensure the optionally provided "sync" servo_motor peripheral has been specified.
160
    if hasattr(self,'sync'):
161
      for p in config.peripheral:
162
        if (str(p.__class__) == str(self.__class__)) and (p.outsignal == self.sync):
163
          break;
164
      else:
165
        raise SSBCCException('Can\'t find preceding servo_motor peripheral with outsignal=%s at %s ' % (self.sync,loc,));
166
      if not hasattr(p,'period'):
167
        raise SSBCCException('servo_motor peripherial with outsignal=%s must have period specified to be used at %s' % (self.sync,loc,));
168
    # Translate the outsignal specification into a single member for the signal
169
    # name and a specification as to whether or not the signal is inverted.
170
    if hasattr(self,'outsignaln'):
171
      self.outsignal = self.outsignaln;
172
      self.invertOutsignal = True;
173
    else:
174
      self.invertOutsignal = False;
175
    # Set the string used to identify signals associated with this peripheral.
176
    self.namestring = self.outsignal;
177
    # Calculate the name of the signal to start the PWM.
178
    self.periodSignal = 's__%s__period_done' % (self.namestring if hasattr(self,'period') else self.sync)
179
    # Calculate the scaling and set the optionally specified constants.
180
    # TODO -- ensure the realizable min_width is positive
181
    self.scaleValue = int(math.ceil((self.max_width-self.min_width)*self.freq_hz/2**config.Get('data_width')));
182
    self.scale_maxValue = int(math.ceil((self.max_width-self.min_width)*self.freq_hz/self.scaleValue));
183
    for scalingPair in (
184
      ( 'scaling',      'scaleValue',           ),
185
      ( 'scale_max',    'scale_maxValue',       ),
186
    ):
187
      if hasattr(self,scalingPair[0]):
188
        config.AddConstant(scalingPair[1], getAttr(self,scalingPair[1]));
189
    # Add the I/O port, internal signals, and the INPORT and OUTPORT symbols for this peripheral.
190
    config.AddIO(self.outsignal,1,'output',loc);
191
    if hasattr(self,'period'):
192
      config.AddSignal(self.periodSignal, 1, loc);
193
    self.ix_outport = config.NOutports();
194
    config.AddOutport((self.outport,
195
                       False,
196
                       # empty list
197
                      ),loc);
198
    if hasattr(self,'inperiod'):
199
      config.AddSignal('s_SETRESET_%s' % self.periodSignal,1,loc);
200
      config.AddInport((self.inperiod,
201
                        (self.periodSignal, 1, 'set-reset',),
202
                       ),loc);
203
 
204
  def GenVerilog(self,fp,config):
205
    body = self.LoadCore(self.peripheralFile,'.v');
206
    if hasattr(self,'period'):
207
      body = re.sub(r'@PERIOD_BEGIN@\n','',body);
208
      body = re.sub(r'@PERIOD_END@\n','',body);
209
    else:
210
      body = re.sub(r'@PERIOD_BEGIN@.*?@PERIOD_END@\n','',body,flags=re.DOTALL);
211
    nbits_scale = CeilLog2(self.scaleValue);
212
    if nbits_scale == 0:
213
      body = re.sub(r'@SCALE_0_BEGIN@\n','',body);
214
      body = re.sub(r'@SCALE_0_ELSE@.*?@SCALE_0_END@\n','',body,flags=re.DOTALL);
215
    else:
216
      body = re.sub(r'@SCALE_0_BEGIN@.*?@SCALE_0_ELSE@\n','',body,flags=re.DOTALL);
217
      body = re.sub(r'@SCALE_0_END@\n','',body);
218
    scaled_min_width = int(math.floor(self.min_width*self.freq_hz/self.scaleValue));
219
    scaled_default_width = int(math.floor(self.default_width*self.freq_hz/self.scaleValue));
220
    scaled_max_width = int(math.floor(self.max_width*self.freq_hz/self.scaleValue));
221
    nbits_pwm = max(config.Get('data_width')+1,CeilLog2(scaled_max_width));
222
    pwm_formula = "%d'd%d + { %d'd0, s_N }" % (nbits_pwm,scaled_min_width-1,nbits_pwm-8,);
223
    if hasattr(self,'period'):
224
      period = self.period * self.freq_hz / self.scaleValue;
225
      nbits_period = CeilLog2(period);
226
    else:
227
      period = 1;
228
      nbits_period = 0;
229
    for subpair in (
230
      ( r'@DEFAULT_PWM@',       "%d'd%d" % (nbits_pwm,scaled_default_width-1,), ),
231
      ( r'@INVERT@',            '!' if self.invertOutsignal else '',            ),
232
      ( r'@IX_OUTPORT@',        "8'd%d" % self.ix_outport,                      ),
233
      ( r'@NAME@',              self.namestring,                                ),
234
      ( r'@NBITS_PERIOD@',      str(nbits_period),                              ),
235
      ( r'@NBITS_PWM@',         str(nbits_pwm),                                 ),
236
      ( r'@NBITS_SCALE@',       str(nbits_scale),                               ),
237
      ( r'@ONE_PERIOD@',        "%d'd1" % nbits_period,                         ),
238
      ( r'@ONE_PWM@',           "%d'd1" % nbits_pwm,                            ),
239
      ( r'@ONE_SCALE@',         "%d'd1" % nbits_scale,                          ),
240
      ( r'@OUTSIGNAL@',         self.outsignal,                                 ),
241
      ( r'@PERIOD_MINUS_ONE@',  "%d'd%d" % (nbits_period,period-1,),            ),
242
      ( r'@PWM_FORMULA@',       pwm_formula,                                    ),
243
      ( r'@SCALE_MINUS_ONE@',   "%d'd%d" % (nbits_scale,self.scaleValue-1,),    ),
244
      ( r'\bgen__',             'gen__%s__' % self.namestring,                  ),
245
      ( r'\bs__',               's__%s__' % self.namestring,                    ),
246
      ( r'@PERIOD_SIGNAL@',     self.periodSignal,                              ), # must be after ( r'\bs__', ...
247
    ):
248
      body = re.sub(subpair[0],subpair[1],body);
249
    body = self.GenVerilogFinal(config,body);
250
    fp.write(body);

powered by: WebSVN 2.1.0

© copyright 1999-2024 OpenCores.org, equivalent to Oliscience, all rights reserved. OpenCores®, registered trademark.