Code:

import sys
import serial
import time

# The types of packets.
PING       = [0x01]
READ_DATA  = [0x02]
WRITE_DATA = [0x03]
REG_WRITE  = [0x04]
ACTION     = [0x05]
RESET      = [0x06]
SYNC_WRITE = [0x83]

# The various errors that might take place.
ERRORS = {64 : "Instruction",
          32 : "Overload",
          16 : "Checksum",
           8 : "Range",
           4 : "Overheating",
           2 : "AngleLimit",
           1 : "InputVoltage"}

def _Checksum(s):
  """Calculate the Dynamixel checksum (~(ID + length + ...)) & 0xFF."""
  return (~sum(s)) & 0xFF

def _VerifyID(id):
  """
  Just make sure the id is valid.
  """
  if not (0 <= id <= 0xFD):
    raise ValueError, "ID %d isn't legal!" % id

def _EnWire(v):
  """
  Convert an int to the on-wire (little-endian) format. Actually returns the
  list [lsbyte, msbyte]. Of course, this is only useful for the 16-bit
  quantities we need to deal with.
  """
  if not 0 <= v <1023>> 8]

def _DeWire(v):
  """
  Invert EnWire. v should be the list [lsbyte, msbyte].
  """
  return (v[1] <<8> 0:
        res.append(self.port.read())
      count += 1
      print count
    return Response(map(ord, res)).Verify()

  # From here on out, you're looking at functions that really do something to
  # the servo itself. You should look at the user manual for details on what
  # all of these mean, although most are self-explanatory.
  def Reset(self, id):
    """
    Perform a reset on the servo. Note that this will reset the ID to 1, which
    could be messy if you have many servos plugged in.
    """
    _VerifyID(id)
    self.Interact(id, RESET).Verify()

  def GetPosition(self, id):
    """
    Return the current position of the servo. See the user manual, page 16,
    for what the return value means.
    """
    _VerifyID(id)
    packet = READ_DATA + [0x24] + [2]
    res = self.Interact(id, packet).Verify()
    if len(res.parameters) != 2:
      raise ValueError, "GetPosition didn't get two parameters!"
    return _DeWire(res.parameters)

  def GetPositionDegrees(self, id):
    """
    If you'd rather work in degrees, use this one. Again, see the user manual,
    page 16, for details.
    """
    return self.GetPosition(id) * (300.0 / 1023.0)

  def SetPosition(self, id, position):
    """
    Set servo id to be at position position. See the user manual, page 16, for
    how this works. This just sends the set position packet; the servo won't
    necessarily go where you told it. You can use GetPosition to figure out
    where it actually went.
    """
    _VerifyID(id)
    if not (0 <= position <= 1023):
      raise ValueError, "Invalid position!"
    packet = WRITE_DATA + [0x1e] + _EnWire(position)
    self.Interact(id, packet).Verify()

  def SetPositionDegrees(self, id, deg):
    """
    Set the position in degrees, according to the diagram in the manual on
    page 16.
    """
    if not 0 <= deg <= 300:
      raise ValueError, "%d is not a valid angle!" % deg
    self.SetPosition(id, int(1023.0/300 * deg))

  def SetID(self, id, nid):
    """
    Change the ID of a servo. Note that this is persistent; you may also be
    interested in Reset().
    """
    _VerifyID(id)
    if not 0 <= nid <= 253:
      raise ValueError, "%id is not a valid servo ID!" % nid
    packet = WRITE_DATA + [0x03] + [nid]
    self.Interact(id, packet).Verify()
 
  def GetMovingSpeed(self, id):
    """
    Get the moving speed. 0 means "unlimited".
    """
    _VerifyID(id)
    packet = READ_DATA + [0x20] + [2]
    Q = self.Interact(id, packet).Verify()
    if len(Q.parameters) != 2:
      raise ValueError, "GetMovingSpeed has the wrong return shape!"
    return _DeWire(Q.parameters)

  def SetMovingSpeed(self, id, speed):
    """
    Set the moving speed. 0 means "unlimited", so the servo will move as fast
    as it can.
    """
    _VerifyID(id)
    if not 0 <= speed <= 1023:
      raise ValueError, "%d is not a valid moving speed!" % speed
    packet = WRITE_DATA + [0x20] + _EnWire(speed)
    self.Interact(id, packet).Verify()

  def Moving(self, id):
    """
    Return True if the servo is currently moving, False otherwise.
    """
    _VerifyID(id)
    packet = READ_DATA + [0x2e] + [1]
    Q = self.Interact(id, packet).Verify()
    return Q.parameters[0] == 1

  def WaitUntilStopped(self, id):
    """
    Spinlock until the servo has stopped moving.
    """
    while self.Moving(id):
      pass

# Handy for interactive testing.
if __name__ == "__main__":
  if len(sys.argv) != 1:  # Specifying a port for interactive use
    ps = sys.argv[1]
  else:
    ps = "/dev/cu.usbserial-A4008bbH"
  X = ServoController(ps)
  X.SetPosition(7,512) # Set servo ID 1 to position 512 (straight up)
  X.WaitUntilStopped(7)
  X.SetPosition(7,1)
  #X.SetPosition(8,1023)


Statistics: Posted by wolf — Fri Jul 13, 2012 10:46 pm