読者です 読者をやめる 読者になる 読者になる

cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

LAUNCHXL-F28069MとBOOSTXL-DRV8305EVMでSCIを使うとき、問題を避けるためにADCの割り込み優先順位を上げる方法

TiのC2000Piccoloの割り込みは、独自のPIEテーブルとかいうものに基づいたもので、とても分かりにくく使いにくいです。
C2000シリーズではPIEテーブルの順序の変更も容易ではなく(掲示板にはデキナイ、との書き込みも)、さらにその定義も小首を傾げたくなるような順序に並んでいます。
InstaSPINモータドライバを使うとき、ADCがフル稼働しているのですが、標準だとこの優先順位がSCI(UART)よりも低く設定されているため、SCI割り込みが入るとモータが動かせないという謎仕様です。
ただし、これは問題を知っていれば簡単に回避できます。

割り込みトリガーをADC_IntNumber_1からADC_IntNumber_1HP(High Priority)に変更

なぜか、同じADCに優先順位の設定が2つ割り当てられていて、10番(標準)と1番(優先)です。
デフォルトで10番に割り当てられているものを1番のPIEで駆動されるように変更します。

hal.c

void HAL_enableAdcInts(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;

  // enable the PIE interrupts associated with the ADC interrupts
  PIE_enableAdcInt(obj->pieHandle,ADC_IntNumber_1HP); // ADC_IntNumber_1 to ADC_IntNumber_1HP

  // enable the ADC interrupts
  ADC_enableInt(obj->adcHandle,ADC_IntNumber_1);

  // enable the cpu interrupt for ADC interrupts
  CPU_enableInt(obj->cpuHandle,CPU_IntNumber_1); // CPU_IntNumber_10 to CPU_IntNumber_1

  return;
} // end of HAL_enableAdcInts() function

hal.h

static inline void HAL_initIntVectorTable(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;
  PIE_Obj *pie = (PIE_Obj *)obj->pieHandle;

  ENABLE_PROTECTED_REGISTER_WRITE_MODE;

  pie->ADCINT1_HP = &mainISR; // pie->ADCINT1 to pie->ADCINT1_HP
  //pie->SCIRXINTA = &sciarxISR;

  DISABLE_PROTECTED_REGISTER_WRITE_MODE;

  return;
} // end of HAL_initIntVectorTable() function

hal.h

static inline void HAL_acqAdcInt(HAL_Handle handle,const ADC_IntNumber_e intNumber)
{
  HAL_Obj *obj = (HAL_Obj *)handle;

  // clear the ADC interrupt flag
  ADC_clearIntFlag(obj->adcHandle,intNumber);

  // Acknowledge interrupt from PIE group 1
  PIE_clearInt(obj->pieHandle,PIE_GroupNumber_1); // PIE_GroupNumber_10 to PIE_GroupNumber_1

  return;
} // end of HAL_acqAdcInt() function

これでも、SCITX割り込みがうまく動かない。なんでた”~。

SLAM実験に便利そうな移動ロボット用プラットフォーム

筑波大の油田研の移動ロボットの研究成果を元に、ハードウェアやソフトウェアが整備されているそうです。
現在は、東北大の渡辺先生を中心にメンテナンスされているとのことです。

ソフトウェア

openspurはskid型の移動ロボットを座標指令で動かしたいときに便利そうなライブラリです。
Spurはシュプールと発音するそうです。
OpenSpur.org

Google グループ

YP-Spur [Robot Platform Project Wiki]

ハードウェア

エンコーダ付きACモータや、モータドライバなどのハードウェアを購入できるようになっています。
t-frog.com

ROS連携なども整備されているようですから、
つくばチャレンジ的な本格的な実験や、ちょっとしたSLAMの実験にも便利に使えそうです。
論文や資料も充実していますので、勉強のための教科書的に一度目を通しておくと良さそうです。

ただ願わくば、
もう少し明確にハードウェアとソフトウェアの切り分けをして欲しかった。

Emlid Navio2で動かすardupilotにもzeroMQを導入する

navio2はEmlid社の開発するRaspberry Piベースのオートパイロット用ボードで、オープンソースのドローンシステムardupilotに対応しています。
emlid.com
詳細はこちら。
Navio2 docs

まず、マニュアルに従ってAPMを普通にインストールしてください。
ここでは、planeでもcopterでもなくroverをターゲットとすることを前提にします。

基本的にRCサーボモータを出力対象として設計されていますので、ロボット用コマンドサーボやもっと大型の汎用モータを動かすためにはインターフェースを自分で用意してやる必要があります。
navio2ボード上にI2CやUARTのコネクタがありますので、それを使って出力を取っても良いですし、RaspberryPiのUSBやLANも使えますのでそれらを使って外部のデバイスとつなげても良いです。

ただ、あらゆるデバイスはzeroMQで繋げてしまえばいいじゃん、という思想があるので、このページではardupilotでzeroMQを使う方法を調査します。

準備

1. wafを見てみる

新しいビルドシステムwafで、ライブラリを追加する方法がわからない。
[1]を勉強して、

def build(bld):
bld(...
lib = ['m'],
libpath = ['/usr/lib'],
...)

のように書けばいいのだろうな、などと推測はつくものの、
ardupilotのwscriptが大きくて、あちらこちらに散在するので、何がどういう順番で呼ばれるのかよくわからない。
*1

2. Configure/Makeを見てみる

> cd ardupilot
> make -C APMrover2 navio2

で試してみるとまだ使える。廃止されると書かれているが。こちらなら使い慣れていてどこに何を書けばいいのかもわかるので、当面こちらで乗り切ることにする。

3. zeroMQのインストール

zmqは普通にライブラリをインストールしてください。ソースからが無難だと思います。
c++用のbindingを/usr/local/includeに置いてください。

Makefileの編集

ardupilotのmakefileが存在する場所は、~/ardupilot/mkの下です。
zeroMQを追加するときに編集するべきファイルは、board_native.mkです。

CXXOPTS = -ffunction-sections -fdata-sections -fno-exceptions -fsigned-char

と書かれた一文を探して、zmqを使うのに都合がいいように次のように変更します。

CXXOPTS = -ffunction-sections -fdata-sections -fexceptions -fsigned-char
LIBS += -lzmq -lpthread

no-exceptionsを外してしまったので、予期せずエラー時の挙動が何か変わってしまうかもしれません。注意してください。
とりあえず、これでbuildは通るようになります。

モータの出力をpubで垂れ流す準備

APMrover2/Rover.hにコンテキストとソケットの定義を追加

#include <zmq.hpp> // 追加

class Rover
{
・・・

public:
    zmq::context_t *context;
    zmq::socket_t *publisher;
};

APMrover2/Rover.cppのRoverコンストラクタ内で初期化

Rover::Rover(void) :
・・・
{
    context = new zmq::context_t(1);
    publisher = new zmq::socket_t(*context, ZMQ_PUB);
    int v = 1;
    size_t size = sizeof(v);
    publisher->setsockopt(ZMQ_SNDHWM, &v, size);
    publisher->bind("tcp://*:6636"); // 送信先IPとポート番号 
}

RCサーボモータへの出力をzmqで流す

出力っぽい名前の関数呼び出しを探していくと、サーボモータへの出力はStering.cpp内で行われてることが見つかります。

void Rover::set_servos(void){
・・・
channel_steer->output();
channel_throttle->output();
・・・
}

ここのPWM出力していると思しき値の、元になった値をもらって来ればよさそうです。

channel_steerもchannel_throttleもRC_Channelというクラスのインスタンスのようなので、RC_Channelのクラスの定義を探しますが、APMrover2の中には見当たりません。
探すと、~/ardupilot/libraries/RC_Channelの下に関連ファイルがありそうです。
RC_Channel.hの中を見ていくと、

int_16t get_servo_out() const { return _servo_out; }
void set_servo_out(int_16t val) {_servo_out = val; }

というのがあり、RC_Channel.cppの中でoutput()から辿ってcalc_pwmの中を見ていくと、range_to_pwm()などの中で_servo_out変数が随所で使われていますので、とりあえずコレで良さそうです。
レンジをpwm用に整形された値を得たい場合は、get_pwm_out()関数を使います。

ということで、
Stering.cppのser_servos(void)関数の中に、zeroMQ出力を追加します。

channel_steer->output();
channel_throttle->output();

int16_t l = channel_steer->get_servo_out();
int16_t r = channel_throttle->get_servo_out();

int msg_size = 20;
zmq::message_t msg(msg_size);
memset (msg.data (), 0, msg_size);
snprintf((char*)msg.data(), msg_size,
"/servo_out %d %d", l, r);
publisher->send(msg);

これでひとまず行けそうです。

お片付け

行儀よくデストラクタで、contextとsocketを削除するようにしないといけません。
Rover::~Rover(void);の定義をRover.hの中に追加して、

Rover::~Rover(void)
{
    if(publisher){
         int v = 0;
         size_t size = sizeof(v);
         publisher->setsockopt(ZMQ_LINGER, &v, size);
         // publisher->close();
         delete publisher;
    }

    if(context){
         // context->close();
         delete context;
    }
}

とします。

実行は、別のコントローラ用PCを用意してAPMPlanner2などを立ち上げておいてから、
raspberrypi側で、

> cd ardupilot/APMrover2
> sudo ./APMrover2.elf -A udp:192.168.10.41:14550

のようにします。14550は規定のport番号、192.168.10.41はコントローラ用PCのIPアドレスの例です。

これとは別に、zeromqのsubを受け取るプログラムを書いて受け取ってみると、

/servo_out -15796 -4

といった感じでデータが送られてきていることが分かります。

zeroMQで繋がってさえしまえば、あとはもう自由に料理できますね!

追記

最近、ソースコードをダウンロードした場合、mavgenに必要なpythonライブラリが正しくインストールされなくてエラーが出ます。
解決方法は、こちらのページを参照してください。
ArduPilot 入門 (7):ビルドが失敗する、シミュレーターが起動しない場合のトラブルシューティング – Drone Japan

> sudo pip install future
> sudo pip install lxml


[1] wafチュートリアル
waf チュートリアル - 純粋関数型雑記帳

*1:プログラムをビルドをするためだけに、また大きなプログラムを書かなければいけない本末転倒な仕組みな気がするので、wafは今のところ評価できない。 そのうち、wafを書くためだけのツールが出てきそうな気がする。

pyzmqとpyOSCでOSC over ZeroMQっぽいもの

普段はc++ばかりなのですが、少しだけpythonで書く必要ができました。
極力、普段と同じようにぬるく書きたいので、使い慣れたZeroMQとOSCを使えるようにしたいと思います。

1.準備

zeromqとoscをpipでインストールします。

> pip install pyzmq
> pip install pyosc

2.テストプログラム

[2][3]のexamplesと[1]のpyOSCの実装を参考に、OSCMessageのbinaryをzeroMQを通して送受信するサンプルを書きたいと思います。

実装のポイント

OSCMessageをバイナリ形式で送受信することだけを考えます。
どの環境の、どのOSCの実装でもネットワーク上を飛び交うパケットは同じバイナリのはず、なので。

OSCMessageのバイナリを取得する方法は、普通に書くだけです。
送られてきたバイナリ形式のOSCMessageを、OSCMessageに戻すには(効率悪いですが、手を抜いて)

recv = decodeOSC(recv_binary)

で文字列や数値にデコードした結果を元に、

recv_osc = OSCMessage(recv[0])
recv_osc += recv[2:]

OSCMessageを作り直しているだけです。イベントハンドラ等は自分で好きに書いてください。

ソースコード

req.py

import zmq
from OSC import decodeOSC, OSCMessage

ctx = zmq.Context()
sock = ctx.socket(zmq.REQ)
sock.connect('tcp://127.0.0.1:5555')

# Prepairing an OSCMessage
msg = OSCMessage("/my/osc/address")
msg.append('something')
msg.insert(0, 'something else')
msg[1] = 'entirely'
msg.extend([1,2,3.])
msg += [4,5,6]
del msg[3:6]
msg.pop(-2)
print 'send'
print 'OSCMessage:\t', msg
print 'OSCMessage(binary):\t', msg.getBinary()

# Send the OSCMessage
sock.send(msg.getBinary())

# Receive the echo
recv_binary = sock.recv()
print 'echo'
print 'binary:\t', recv_binary
recv = decodeOSC(recv_binary)
print 'string:\t', recv
recv_osc = OSCMessage(recv[0])
recv_osc += recv[2:]
print 'OSCMessage:\t', recv_osc
print 'OSCMessage(binary):\t', recv_osc.getBinary()

rep.py

import zmq
from zmq.eventloop import ioloop
from OSC import decodeOSC, OSCMessage

loop = ioloop.IOLoop.instance()

ctx = zmq.Context()
sock = ctx.socket(zmq.REP)
sock.bind('tcp://127.0.0.1:5555')

def rep_handler(sock, events):
  # Receive an OSC packet
  osc_binary = sock.recv()
  print 'received'
  print 'binary:\t', osc_binary
  # Decode the binary to OSCMessage
  msg = decodeOSC(osc_binary)
  osc_msg = OSCMessage(msg[0])
  osc_msg += msg[2:]
  print 'OSCMessage:\t', osc_msg
  # Reply the decoded OSCMessage
  sock.send(osc_msg.getBinary())

loop.add_handler(sock, rep_handler, zmq.POLLIN)

loop.start()

実行結果

> E:\workspace\python\req.py
send
OSCMessage: /my/osc/address ['something else', 'entirely', 1, 6]
OSCMessage(binary): /my/osc/address ,ssii something else entirely
echo
binary: /my/osc/address ,ssii something else entirely
string: ['/my/osc/address', ',ssii', 'something else', 'entirely', 1, 6]
OSCMessage: /my/osc/address ['something else', 'entirely', 1, 6]
OSCMessage(binary): /my/osc/address ,ssii something else entirely

> E:\workspace\python\rep.py
received
binary: /my/osc/address ,ssii something else entirely
OSCMessage: /my/osc/address ['something else', 'entirely', 1, 6]

c++の実装と繋いでみる

cvl-robot.hateblo.jp
ここで昔書いたテストプログラムのpub,subと、pythonで書いたpub,subとを繋いでみます。

sub.py

import zmq
from zmq.eventloop import ioloop
from OSC import decodeOSC, OSCMessage

loop = ioloop.IOLoop.instance()

ctx = zmq.Context()
sock = ctx.socket(zmq.SUB)
sock.connect('tcp://127.0.0.1:5555')

sock.setsockopt(zmq.SUBSCRIBE, '')

def sub_handler(sock, events):
  # Receive an OSC packet
  osc_binary = sock.recv()
  print 'received'
  print 'binary:\t', osc_binary
  # Decode the binary to OSCMessage
  msg = decodeOSC(osc_binary)
  osc_msg = OSCMessage(msg[0])
  osc_msg += msg[2:]
  print 'OSCMessage:\t', osc_msg

loop.add_handler(sock, sub_handler, zmq.POLLIN)

loop.start()

pub.py

import time
import zmq
from OSC import decodeOSC, OSCMessage

ctx = zmq.Context()
sock = ctx.socket(zmq.PUB)
sock.bind('tcp://127.0.0.1:9999')

# Prepairing an OSCMessage
msg = OSCMessage("/my/osc/address")
msg.append('something')
msg.insert(0, 'something else')
msg[1] = 'entirely'
msg.extend([1,2,3.])
msg += [4,5,6]
del msg[3:6]
msg.pop(-2)
print 'send'
print 'OSCMessage:\t', msg
print 'OSCMessage(binary):\t', msg.getBinary()

# Send the OSCMessage
time.sleep(1.0)
sock.send(msg.getBinary())

c++の方も,送受信先のフィルタを

publisher->bind("tcp://*:5555");
subscriber->connect("tcp://127.0.0.1:9999");

適当に調整してください。

実行結果

received
binary: /mouse/button ,siii pressed $ [
OSCMessage: /mouse/button ['pressed', 548, 347, 2]
received
binary: /mouse/button ,siii pressed ( _
OSCMessage: /mouse/button ['pressed', 296, 351, 0]
received
binary: /mouse/button ,siii pressed \
OSCMessage: /mouse/button ['pressed', 260, 604, 0]
received
binary: /key/pressed ,i c
OSCMessage: /key/pressed [99]
received
binary: /key/pressed ,i v
OSCMessage: /key/pressed [118]
received
binary: /key/pressed ,i l
OSCMessage: /key/pressed [108]

Received: /my/osc/address something else entirely 1 6
Received: /my/osc/address something else entirely 1 6

問題なく動きますね。

[1] pythonでOSC Messageのデコード
https://github.com/ptone/pyosc/blob/master/OSC.py
[2] pyzmqのreq,repのexample
zmq の python binding で echo テスト - Twisted Mind
[3] pyOSCのexample
pyosc/examples at master · ptone/pyosc · GitHub

[4] pyzmqのpub, subのexample
pyzmqでZeroMQを触ってみる (PUB/SUB) - YAMAGUCHI::weblog
[5] int64を送受信したいとき
pyOSCでint64が受け取れなくてはまったメモ - Qiita

ロボットには出来ないこと

(編集中)
慣用句として、ロボットには出来ない、という言葉が良くつかわれます。

とくに伝統工芸や芸能などに多いような気がします。
本当に出来ないのかどうか検証したいので、ロボットには出来ない、という言葉を聞いた時にここにメモしておこうと思います。

・ジーンズを作れない

IoT最強プラットフォームを目指して、RaspberryPi3にopenFrameworksをインストールして、ofxZmqをWindow(openGL)無しでも動くようにする(その1)

(編集中)
raspberry Piは安くて早くて、arduinoと違ってネットワークに簡単に繋がる利点があります。
また、ネットワークはzeroMQを使えば、高速・安定・簡単に書けることも覚えてしまいました。
さらに、openFrameworksは様々な用途のライブラリaddonをいろいろな人が開発していて、様々な用途に便利に使えます。

そこで、raspberry Piの上のopenFrameworksでzeroMQが使える環境を整えておきたいと思います。
バイス制御などの用途では、GUI Windowがあることが却ってネックになってしまうことがあるので、
サーバー向けにGUI無しでも動かせるようにしておきます。

1. 最新のRaspbian(jessie)を普通にインストールする.

2. openFrameworksのソースをダウンロードしてインストールする.

このページを参考にします。
raspberry pi | openFrameworks
ARMv6版とARMv7版が選択肢にありますが、Raspbianではv6、Armlinuxではv7を選ぶとよいようです。

3. zeromqのcpp bindingsをダウンロードして/usr/includeにコピー

http://zeromq.org/bindings:cpp

> cd /usr/include/
> sudo wget https://raw.githubusercontent.com/zeromq/cppzmq/master/zmq.hpp

4. 作者に感謝しながら、ofxZmqをダウンロード

cd /home/pi/openFrameworks/addons
wget --no-check-certificate https://github.com/satoruhiga/ofxZmq/archive/master.tar.gz
tar xvzf master.tar.gz
mv ofxZmq-master ofxZmq
rm master.tar.gz

5. sodiumをインストール

[1]を参考に。

sudo apt-get install libsodium-dev

6. 空の新規プロジェクトを作る

RaspberryPiでは、残念ながらプロジェクトジェネレータを使えないそうなので、ファイルコピーで空のプロジェクトを用意します。

cp -R /home/pi/openFrameworks/examples/empty/emptyExample /home/pi/openFrameworks/apps/myApps/myRpiApp
cd /home/pi/openFrameworks/apps/myApps/myRpiApp
make
make run

7. ofxZmqをプロジェクトに追加するためaddons.makeを編集。

> cd ~/openFrameworks/apps/myApps/myRpiApp
> nano addons.make

ofxZmq

8. sodiumをリンクするようにconfig.makeを編集

PROJECT_LDFLAGSの行を探して、コメントアウトを解除。

PROJECT_LDFLAGS=-Wl,-rpath=./libs -lsodium

9. ofxZmqのexampleのソースコードをコピーして持ってくる

cd ~/openFrameworks/apps/myApps/myRpiApp/src
cp /home/pi/openFrameworks/addons/ofxZmq/example/src/testApp.cpp ./
cp /home/pi/openFrameworks/addons/ofxZmq/example/src/testApp.h ./

main.cppを次のように編集します。

#include "ofMain.h"

//define _NO_WINDOW
#ifdef _NO_WINDOW
#include "ofAppNoWindow.h"
#endif // _NO_WINDOW

#include "testApp.h"

int main(int argc, char* argv[])
{
#ifndef _NO_WINDOW
    ofSetupOpenGL(1024, 768, OF_WINDOW);
#else
    ofAppNoWindow window;
    ofSetupOpenGL(window, 0, 0, OF_WINDOW);
#endif

    ofRunApp(new testApp());

    ruturn 0;
}

もしくは、

ofSetFramerate(0);

をしておく。

10. platform.hppにZMQ_USE_EPOLLの定義を追加

nano ~/openFrameworks/addons/ofxZmq/libs/zmq/src/platform.hpp

#define ZMQ_USE_EPOLL
#define ZMQ_HAVE_UIO 1

ZMQ_HAVE_UIOはstruct iovecが多重定義になるのを防ぐのに必要だとのこと。

コンパイルを通すために適当に立てたフラグなので、環境に合わせてちゃんといろいろ設定しないとダメかもしれない。[2]を参考に。要確認。

11. makeして実行

> make
> make run

GUIなしにすると、キー入力も拾ってもらえませんので、testApp.cppを適当に書き換える必要があります。


これで、IoT最強に成れるのではないだろうか!

[1] osdevlab.blogspot.jp
[2] github.com
[3] qiita.com

モバイルバッテリー、有名メーカーの製品が良いと思い込んでいると時代に置いて行かれるかも?

Ankerの大容量モバイルバッテリーが良い、という記事を読んだ。
www.gurinovation.com
まさしくこの機種Anker PowerCore 20100を購入して使っていたので悪くない製品だということは知っているのだが、購入して1年ちょっと経った今、へたってしまってただ大きくて重い物体になってしまっている。

安価なので買い替えてもいいのだが、トランジスタ技術2017年1月号に気になる記事が載っている。
トランジスタ技術 2017年 1月号

151~157ページ、大塚康二さんの書かれた全力実験!No.1モバイルバッテリを探せ!の中で、いくつかのバッテリーの寿命テストとランキングが紹介されている。
詳細は載せられないが、ここで一番だったのは全然知らない中華メーカーRockの製品で、サイクル寿命が長いことが絶賛されていた。

2位はソニーのCP-10A、3位はAnker5。

何が言いたいのかというと、
リチウムイオン電池の性能は日本製が一番!やアメリカが強い!などと思い込んでいると、電池は電子機器の基幹部品なので、あっという間に中国に置いて行かれて没落してしまう危機にあるのではないかということ。