stepper.py

   1 # Python control of stepper motors.
   2
   3 # Copyright (C) 2008-2011  W. Trevor King
   4 #
   5 # This program is free software: you can redistribute it and/or modify
   6 # it under the terms of the GNU General Public License as published by
   7 # the Free Software Foundation, either version 3 of the License, or
   8 # (at your option) any later version.
   9 #
  10 # This program is distributed in the hope that it will be useful,
  11 # but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 # GNU General Public License for more details.
  14 #
  15 # You should have received a copy of the GNU General Public License
  16 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17
  18 import logging as _logging
  19 import logging.handlers as _logging_handlers
  20 from time import sleep as _sleep
  21
  22
  23 __version__ = '0.3'
  24
  25
  26 LOG = _logging.getLogger('stepper')
  27 "Stepper logger"
  28
  29 LOG.setLevel(_logging.WARN)
  30 _formatter = _logging.Formatter(
  31     '%(asctime)s - %(name)s - %(levelname)s - %(message)s')
  32
  33 _stream_handler = _logging.StreamHandler()
  34 _stream_handler.setLevel(_logging.DEBUG)
  35 _stream_handler.setFormatter(_formatter)
  36 LOG.addHandler(_stream_handler)
  37
  38
  39 def _binary(i, width=4):
  40     """Convert `i` to a binary string of width `width`.
  41
  42     >>> _binary(0)
  43     '0000'
  44     >>> _binary(1)
  45     '0001'
  46     """
  47     str = bin(i)[2:]
  48     return '0'*(width-len(str)) + str
  49
  50
  51 class Stepper (object):
  52     """Stepper motor control
  53
  54     inputs:
  55
  56     write      (fn(value)) write the 4-bit integer `value` to
  57                appropriate digital output channels.
  58     full_step  (boolean) select full or half stepping
  59     logic      (boolean) select active high (True) or active low (False)
  60     delay      (float) time delay between steps in seconds, in case the
  61                 motor response is slower than the drigital output
  62                 driver.
  63     step_size  (float) approximate step size in meters
  64     backlash   (int) generous estimate of backlash in half-steps
  65
  66     >>> from pycomedi.device import Device
  67     >>> from pycomedi.channel import DigitalChannel
  68     >>> from pycomedi.constant import SUBDEVICE_TYPE, IO_DIRECTION
  69
  70     >>> device = Device('/dev/comedi0')
  71     >>> device.open()
  72
  73     >>> subdevice = device.find_subdevice_by_type(SUBDEVICE_TYPE.dio)
  74     >>> channels = [subdevice.channel(i, factory=DigitalChannel)
  75     ...             for i in (0, 1, 2, 3)]
  76     >>> for chan in channels:
  77     ...     chan.dio_config(IO_DIRECTION.output)
  78
  79     >>> def write(value):
  80     ...     subdevice.dio_bitfield(bits=value, write_mask=2**4-1)
  81
  82     >>> s = Stepper(write=write)
  83     >>> s.position
  84     0
  85     >>> s.single_step(1)
  86     >>> s.position
  87     2
  88     >>> s.single_step(1)
  89     >>> s.position
  90     4
  91     >>> s.single_step(1)
  92     >>> s.position
  93     6
  94     >>> s.single_step(-1)
  95     >>> s.position
  96     4
  97     >>> s.single_step(-1)
  98     >>> s.position
  99     2
 100     >>> s.single_step(-1)
 101     >>> s.position
 102     0
 103     >>> s.full_step = False
 104     >>> s.single_step(-1)
 105     >>> s.position
 106     -1
 107     >>> s.single_step(-1)
 108     >>> s.position
 109     -2
 110     >>> s.single_step(-1)
 111     >>> s.position
 112     -3
 113     >>> s.single_step(1)
 114     >>> s.position
 115     -2
 116     >>> s.single_step(1)
 117     >>> s.position
 118     -1
 119     >>> s.single_step(1)
 120     >>> s.position
 121     0
 122     >>> s.step_to(1000)
 123     >>> s.position
 124     1000
 125     >>> s.step_to(-1000)
 126     >>> s.position
 127     -1000
 128     >>> s.step_relative(1000)
 129     >>> s.position
 130     0
 131
 132     >>> device.close()
 133     """
 134     def __init__(self, write, full_step=True, logic=True, delay=1e-5,
 135                  step_size=170e-9, backlash=100):
 136         self._write = write
 137         self.full_step = full_step
 138         self.logic = logic
 139         self.delay = delay
 140         self.step_size = step_size
 141         self.backlash = backlash
 142         self.port_values = [1,  # binary ---1  setup for logic == True
 143                             5,  # binary -1-1
 144                             4,  # binary -1--
 145                             6,  # binary -11-
 146                             2,  # binary --1-
 147                             10, # binary 1-1-
 148                             8,  # binary 1---
 149                             9]  # binary 1--1
 150         self._set_position(0)
 151
 152     def _get_output(self, position):
 153         """Get the port value that places the stepper in `position`.
 154
 155         >>> s = Stepper(write=lambda value: value, logic=True)
 156         >>> _binary(s._get_output(0))
 157         '0001'
 158         >>> _binary(s._get_output(1))
 159         '0101'
 160         >>> _binary(s._get_output(2))
 161         '0100'
 162         >>> _binary(s._get_output(-79))
 163         '0101'
 164         >>> _binary(s._get_output(81))
 165         '0101'
 166         >>> s.logic = False
 167         >>> _binary(s._get_output(0))
 168         '1110'
 169         >>> _binary(s._get_output(1))
 170         '1010'
 171         >>> _binary(s._get_output(2))
 172         '1011'
 173         """
 174         value = self.port_values[position % len(self.port_values)]
 175         if not self.logic:
 176             value = 2**4 - 1 - value
 177         return value
 178
 179     def _set_position(self, position):
 180         self.position = position  # current stepper index in half steps
 181         output = self._get_output(position)
 182         LOG.debug('set postition to %d (%s)' % (position, _binary(output)))
 183         self._write(output)
 184
 185     def single_step(self, direction):
 186         LOG.debug('single step')
 187         if self.full_step and self.position % 2 == 1:
 188             self.position -= 1  # round down to a full step
 189         if direction > 0:
 190             step = 1
 191         elif direction < 0:
 192             step = -1
 193         else:
 194             raise ValueError(direction)  # no step
 195         if self.full_step:
 196             step *= 2
 197         self._set_position(self.position + step)
 198         if self.delay > 0:
 199             _sleep(self.delay)
 200
 201     def step_to(self, target_position):
 202         if target_position != int(target_position):
 203             raise ValueError(
 204                 'target_position %s must be an int' % target_position)
 205         if self.full_step and target_position % 2 == 1:
 206             target_position -= 1  # round down to a full step
 207         if target_position > self.position:
 208             direction = 1
 209         else:
 210             direction = -1
 211         while self.position != target_position:
 212             LOG.debug('stepping %s -> %s (%s)' % (target_position, self.position, direction))
 213             self.single_step(direction)
 214
 215     def step_relative(self, relative_target_position, backlash_safe=False):
 216         """Step relative to the current position.
 217
 218         If `backlash_safe` is `True` and `relative_target_position` is
 219         negative, step back an additional `.backlash` half-steps and
 220         then come back to the target position.  This takes the slack
 221         out of the drive chain and ensures that you actually do move
 222         back to the target location.  Note that as the drive chain
 223         loosens up after the motion completes, the stepper position
 224         will creep forward again.
 225         """
 226         target = self.position + relative_target_position
 227         if backlash_safe and relative_target_position < 0:
 228             self.step_to(target - self.backlash)
 229         self.step_to(target)