用MSN控制伺服电机

 

测试通过MSN控制ServoMotor基本成功，注意一定要root用户，另外MSNP的charset为utf－8

条件：Phidgets硬件（伺服接口板，伺服电机，也可以使用Arduino实现）
   伺服电机控制端，可以是PC或单板机（具有USB HOST）
   MSNP（MSN的python实现）
   Phidgets驱动和PythonPhidgets（Phidgets的python实现）
   PhidgetsMsnContoller.py(自己的程序）

示意图如下：

   
  
基本代码如下：
#Simple control protocol:

#Command: ctype, para1, para2
# get board attach status:  ctype    ex: 0  (get servo board attach status)
# get motor engage status:  ctype, index   ex: 1, 0 (get servo motor index 0 engage status)
# get position status:   ctype, index   ex: 2, 0 (get servo motor index 0 postion status)
# set position status:   ctype, index, pos ex: 3, 0, 100  (servo motor index 0 move to position 100)

#Status: stype, para1, para2
# board attach/detach status:  stype, val  ex: 0, 0, 0/1 (servo board detached/attached)
# motor engage/disengage status:  stype, index, val ex: 1, 0, 0/1 (servo motor index 0 disengage/engage)
# position status:   stype, index, pos  ex: 2, 0, 100 (servo motor index 0 moved to position 100)

#Basic imports
from ctypes import *
import sys
from time import sleep

#Phidget specific imports
from Phidgets.PhidgetException import *
from Phidgets.Events.Events import *
from Phidgets.Devices.Servo import Servo

#Msn imports
import msnp

class MsnChatListener(msnp.ChatCallbacks):
 def message_received(self, passport_id, display_name, text, charset): 
  print '%s: %s' % (passport_id, text) 
  self.chat.send_message(text, charset)
  self.sb.ProcessCmd(text)
  
class MsnListener(msnp.SessionCallbacks):
 def __init__(self):
  self.sb = ServoBoard(self)
  
 def chat_started(self, chat): 
  callbacks = MsnChatListener() 
  chat.callbacks = callbacks        
  callbacks.chat = chat 
  callbacks.sb = self.sb
  self.sb.chat = chat
  self.sb.onChat = True
  
 def state_changed(self, state): 
  if state == msnp.States.ONLINE:        
   print 'You are now online.'
   self.online = True
   
   self.sb.open()
   self.sb.attach()
   
  elif state == msnp.States.OFFLINE:        
   print 'You are now offline.'
   self.online = False
   self.sb.close()

class MsnController():
 def beginMsnControl(self):
  self.msnListener = MsnListener()
  msn = msnp.Session(self.msnListener)
  msn.login('xxx@hotmail.com', 'xxx')
  msn.sync_friend_list()
  while True: 
   msn.process(chats = True) 
   sleep(1)
   
########################################################
class ServoBoard:
 def __init__(self, listener):
  #Create an servo object
  self.servo = Servo()
  self.listener = listener
  self.charset = 'utf-8'
  self.onChat = False
  
 def ReportStatus(self, type, para1, para2):
  
  try:
   text = str(type) + "," + str(para1)  + "," + str(para2)
   if(self.onChat):
    self.chat.send_message(text, self.charset)
  except:
       print "Report status failed."
     
     
 def ProcessCmd(self, text):
  try:
   cmd = text.split(',')
   if cmd[0] == '0':  
       if(self.servo.isAttached()):                  
               self.ReportStatus(0, 0, 1)
              else:
               self.ReportStatus(0, 0, 0)
   elif cmd[0] == '1': 
       if(self.servo.getEngaged(int(cmd[1]))):                  
               self.ReportStatus(1, int(cmd[1]), 1)
              else:
               self.ReportStatus(1, int(cmd[1]), 0)            
             
          elif cmd[0] == '2':                           
              self.ReportStatus(2, int(cmd[1]), self.servo.getPosition(int(cmd[1])))
          elif cmd[0] == '3':                           
               self.servo.setPosition(int(cmd[1]), int(cmd[2]))        
          else:
              pass
  except:
      print "Process command failed."
        
 #Information Display Function
 def DisplayDeviceInfo(self):
     print "|------------|----------------------------------|--------------|------------|"
     print "|- Attached -|-              Type              -|- Serial No. -|-  Version -|"
     print "|------------|----------------------------------|--------------|------------|"
     print "|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.servo.isAttached(), self.servo.getDeviceType(), self.servo.getSerialNum(), self.servo.getDeviceVersion())
     print "|------------|----------------------------------|--------------|------------|"
     print "Number of motors: %i" % (self.servo.getMotorCount())
 
 #Event Handler Callback Functions
 def ServoAttached(self, e):
     attached = e.device
     print "Servo %i Attached!" % (attached.getSerialNum())
     self.ReportStatus(0, 0, 1)
    
 def ServoDetached(self, e):
     detached = e.device
     print "Servo %i Detached!" % (detached.getSerialNum())
     self.ReportStatus(0, 0, 0)
    
 def ServoError(self, e):
     print "Phidget Error %i: %s" % (e.eCode, e.description)
 
 def ServoPositionChanged(self, e):
     print "Motor %i Current Position: %f" % (e.index, e.position)
     self.ReportStatus(2, e.index, e.position)
 
 def open(self):     
  #Main Program Code
  try:
      self.servo.setOnAttachHandler(self.ServoAttached)
      self.servo.setOnDetachHandler(self.ServoDetached)
      self.servo.setOnErrorhandler(self.ServoError)
      self.servo.setOnPositionChangeHandler(self.ServoPositionChanged)
  except PhidgetException, e:
      print "Phidget Exception %i: %s" % (e.code, e.message)
      print "Exiting...."
      exit(1)
  
  print "Opening phidget object...."
  
  try:
      self.servo.openPhidget()
  except PhidgetException, e:
      print "Phidget Exception %i: %s" % (e.code, e.message)
      print "Exiting...."
      exit(1)
 
 def attach(self):
  print "Waiting for attach...."
  
  try:
      self.servo.waitForAttach(10000)
  except PhidgetException, e:
      print "Phidget Exception %i: %s" % (e.code, e.message)
      try:
          self.servo.closePhidget()
      except PhidgetException, e:
          print "Phidget Exception %i: %s" % (e.code, e.message)
          print "Exiting...."
          exit(1)
      print "Exiting...."
      exit(1)
  else:
      self.DisplayDeviceInfo()
 
 def close(self):
  print "Closing..."
  
  try:
      self.servo.closePhidget()
  except PhidgetException, e:
      print "Phidget Exception %i: %s" % (e.code, e.message)
      print "Exiting...."
      exit(1)
  
  print "Done."
  exit(0)

########################################################
if __name__ == "__main__":
 mc = MsnController()
 mc.beginMsnControl()  
  
待解决问题：
 不能取得电机是否连接的状态（getEngaged错误, - 是否因为驱动版本或硬件过时引起？changed getMotorStatus and setMotorStatus to getEngaged and setEngaged to reflect changes in the C API
）
 在出错时向PC报告