Ministickコンポーネントの作成

 #!/usr/bin/env python
 # -*- coding: euc-jp -*- 
 import sys
 import time
 import spidev
 
 class ADC:
   def __init__(self):
     self.spi = spidev.SpiDev()
     self.spi.open(0, 0)
 
   def get_value(self, channel):
     sned_ch = [0x00,0x08,0x10,0x18]
     if ((channel > 3) or (channel < 0)):
       return -1
     r = self.spi.xfer2([sned_ch[channel],0,0,0])
     ret = ((r[2] << 6 ) & 0x300) |  ((r[2] << 6) & 0xc0) | ((r[3] >> 2) & 0x3f)
     return ret     
 
   def get_voltage(self, channel):
     ret = self.get_value(channel) * 5.0 / 1024
     return ret     
 
 def main():
   adc = ADC()
   while 1:
     adc1 = adc.get_value(0)
     msg1 = "%1.5fV(%04x)" % ((float(adc1)*5/1024),adc1)
     print msg1,
 
     adc1 = adc.get_value(1)
     msg1 = "%1.5fV(%04x)" % ((float(adc1)*5/1024),adc1)
     print msg1,
 
     adc2 = adc.get_value(2)
     msg2 = "%1.5fV(%04x)" % ((float(adc2)*5/1024),adc2)
     print msg2,
 
     adc3 = adc.get_value(3)
     msg3 = "%1.5fV(%04x)" % ((float(adc3)*5/1024),adc3)
     print msg3,
 
     sys.stdout.write("\n")
     time.sleep(0.5)
 
 if __name__ == '__main__':
   main()

 Linux raspbian-armhf 3.2.27+ #307 PREEMPT Mon Nov 26 23:22:29 GMT 2012 armv6l
 
 The programs included with the Debian GNU/Linux system are free software;
 the exact distribution terms for each program are described in the
 individual files in /usr/share/doc/*/copyright.
 
 Debian GNU/Linux comes with ABSOLUTELY NO WARRANTY, to the extent
 permitted by applicable law.
 Last login: Tue May 14 07:07:04 2013 from dhcpe2078.a02.aist.go.jp
 pi@raspbian-armhf ~ $ vi adc_test.py
 pi@raspbian-armhf ~ $ chmod 755 adc_test.py
 pi@raspbian-armhf ~ $ ./adc_test.py

 # Import RTM module
 import RTC
 import OpenRTM_aist
 
 import math
 import spidev

 class Ministick(OpenRTM_aist.DataFlowComponentBase):
   def __init__(self, manager):
     OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
 
     self._scaling = [1.0]
     self._tread = [0.2]
     self._print_xy = ["NO"]
     self._print_vel = ["NO"]
     self._print_wvel = ["NO"]
     self.x = 0.0
     self.y = 0.0
     self.spi = spidev.SpiDev()
     self.spi.open(0, 0)

  def get_adc(self, channel):
    sned_ch = [0x00,0x08,0x10,0x18]
    if ((channel > 3) or (channel < 0)):
      return -1
    r = self.spi.xfer2([sned_ch[channel],0,0,0])
    ret = ((r[2] << 6 ) & 0x300) |  ((r[2] << 6) & 0xc0) | ((r[3] >> 2) & 0x3f)
    return ret

 dev xy_to_wvel(self, x, y):
   th = math.atan2(y, x)
   v = math.hypot(x, y)
   vl = v * math.cos(th - (math.pi/4.0))
   vr = v * math.sin(th - (math.pi/4.0))
   return (vl, vr)

 def wvel_to_vel2d(self, vl, vr):
   v = (vr + vl) / 2.0
   if v < 0.0:
     w = - (vr - vl) / self._tread[0]
   else:
     w = (vr - vl) / self._tread[0]
   return RTC.Velocity2D(v, 0.0, w)

 def onInitialize(self):
     : 中略
    self.x_offset_v = 0.0
    self.y_offset_v = 0.0
    for i in range(1, 100):
      self.x_offset_v += self.get_adc(0)
      self.y_offset_v += self.get_adc(1)
    self.x_offset_v = self.x_offset_v / 100.0
    self.y_offset_v = self.y_offset_v / 100.0
 
    return RTC.RTC_OK

 def onExecute(self, ec_id):
    self.x = - (self.get_adc(0) - self.x_offset_v) * self._scaling[0] / 1000.0
    self.y = (self.get_adc(1) - self.y_offset_v) * self._scaling[0] / 1000.0
    if self._print_xy[0] != "NO":
      print "(x, y) = ", self.x, self.y
    self._d_pos.data = [self.x, self.y]
    
    self._d_wvel.data = self.xy_to_wvel(self.x, self.y)
    if self._print_wvel[0] != "NO":
      print "(vl, vr) = ", self._d_wvel.data[0], self._d_wvel.data[1]
    self._d_vel.data = self.wvel_to_vel2d(self._d_wvel.data[0],
                                          self._d_wvel.data[1])
    if self._print_vel[0] != "NO":
      print "(vx, va) = ", self._d_vel.data.vx, self._d_vel.data.va
 
    self._posOut.write()
    self._velOut.write()
    self._wvelOut.write()
 
    return RTC.RTC_OK

 $ rtm-naming
 $ ./Ministick.py
 

 pi@raspbian-armhf ~ $ ./adc_text.py
 Traceback (most recent call last):
   File "./adc_text.py", line 47, in <module>
     main()
   File "./adc_text.py", line 25, in main
     adc = ADC()
   File "./adc_text.py", line 10, in __init__
     self.spi.open(0, 0)
 IOError: [Errno 13] Permission denied