Spi用のCライブラリをJAVAから呼び出す

ロボットの5個の加速度センサーをRaspberryPIのSpi1の元にぶら下げたいと、先週末から四苦八苦している。そもそも、Spi1が、デバイス上は、ちゃんと認識させているのだが動かなかった。
結局それは、WiringPiのSpiライブラリ関数、およびそれを元にしたJAVAのpi4jライブラリは、RspberryPIのSpi0をしか使わないことを前提にしているからだった(そうではないという方もいるかもしれないが、私にはそうとしか見えなかった)。PythonのSpiライブラリはSpi0もSpi1も使えるようになっていた。
ここでは、JAVAで統括することが基本なので、どうしたものかと思ったが、結局、WiringPiなどを使わずに、C言語でSpi用のダイナミックライブラリを作成して、それをJNAを使って、JAVAから呼ぶ出すという戦略にした。
Cでは、Spiデバイス、
/dev/spidev0.0
/dev/spidev0.1
/dev/spidev1.0
/dev/spidev1.1
/dev/spidev1.2
から直接読み書きするのがいい。もともと、Raspiは、Spi0しか有効化していないのだが、Spi1を有効化する方法は別の記事に書いておいた。
ライブラリは、
https://raw.githubusercontent.com/raspberrypi/linux/rpi-3.10.y/Documentation/spi/spidev_test.c
のC言語コードspidev_test.cを元に作成した。
Cの共有ライブラリの作成方法については、
http://www.koikikukan.com/archives/2016/10/27-000300.php
を、AVAからCライブラリを呼び出す方法については、
http://qiita.com/kiida/items/9d26b850194fa1a02e67
を参照させていただいた。
ライブラリ用に作成したソースをspi.c(main関数はいらない)とすると、
gcc -fPIC -shared -o libspi.so spi.c
とすれば、ライブラリはできる。
作成したライブラリlibspi.soを以下の場所に置いておく。(バグが多々存在したので公開中止)
libspi.so
ただし、使用される場合は、一切の保証はありませんし、完全に自己責任で使っていただきたい。また、非常に、荒っぽく、当面必要なことを目的に作成したものだということはあらかじめお断りしておく。時間と必要性があったら、もっと精緻なものにしたい。
このライブラリを適当なフォルダにおいて、RasPIのターミナルから、
export LD_LIBRARY_PATH=/to/your/library/
として、
$ sudo ldconfig
を実行させる。ldconfigはいらないかもしれない(適当だな笑、普通やるので癖)。
これは、Cライブラリなので、Cから呼び出すことはできるが、ここではJNAを利用して、JAVAのライブラリのように使う。(JAVAから、Cの実行ファイルを呼び出すのは、オーバーヘッドが大きそうでやめた。)
https://github.com/java-native-access/jna/tree/master/dist
から
jna.jar
をダウンロードして、javaプログラムのライブラリに組み込む。
RasPIのSpi1テャンネルにKXSD9という三軸加速度センサーをぶらせげてスレーブデバイス番号0から読み取るサンプルプログラムを最後のところにおいておく。
データはこんな感じで綺麗に取れる。

JAVAのプログラムは、次のようになる。Spi1には、スレーブ1個しかぶら下げていないので、スレーブ  ピンの操作はしていない。いずれ、たくさんのスレーブをぶら下げた場合の結果は、そのうち報告できるはずだ。
ここで使っている、libspi.soの関数は、
int setupSpi(int ch, int ss, int spd)
void transfer(byte [] tx, int size)
void closeSpi()
だが、他に、
void option_usage(const char *prog)
void setOptions(int argc, char *argv[])
という関数もある。
setupSpiはデバイスを初期化します。引数は、チャンネル番号(0 or 1)とスレーブデバイスポート番号(チャンネル0の場合は、0か1、チャンネル1の場合は、0、1、2のいずれか)、そしてクロックスピードである(20Mくらいまではいけるのではないかと思うが、やったことはない)。
transfer関数は、バイト配列に入っているデータを送信し、結果を入れるバッファにもなっている。Spi特有のデータの「玉突き送受信」を再現している。(別々にもできたのだが・・・・・・)。例えば、2バイト送信する場合、1バイトめにコマンド、その結果が2バイト目に入れられて戻ってくるという感じである。センサーのデータの場合は、1バイトめに軸アドレス、2バイトと3バイトめには結果の12ビットデータが入っていたりするわけである。
closeSpiは、デスクリプタとストリームを閉じる関数。
option_usageとsetOptionsの関数は、元々のspidev_test.cの関数をそのまま入れている感じだ。特に後者は、モードの設定などができるようになっている。この終わりの二つの関数は、使う必要もなく、使ったこともないので、これでいいのかどうかは、よくわからない。

import com.sun.jna.Library;
import com.sun.jna.Native;
import java.util.logging.Level;
import java.util.logging.Logger;
// ここの部分がlibspi.soと接続するおまじない
interface SpiLib extends Library {
    SpiLib INSTANCE = (SpiLib) Native.loadLibrary("spi", SpiLib.class);
    // 使うべき関数を宣言する
    int setupSpi(int ch, int ss, int spd) ;
    void transfer(byte [] tx, int size) ;
    void closeSpi();
}
// staticを多用しているが、mainがスタティックなので
// 止むを得ずそうしているだけで
// 普通にやれば、main関数以外、staticにする必要はない
public class SpiTest {
    static  SpiLib spi = SpiLib.INSTANCE;
    public static void main(String[] args) {
        spi.setupSpi(1, 0, 1000000);
        System.out.println("センサーKXSD9の設定をデバイスに書き込みます");
        byte [] com = new byte[2];
        com[0] = 0x0c;  // address byte
        com[1] = (byte) 0xe3;  // 測りたいところで細かくしている
        spi.transfer(com,2);
        com[0] = 0x0d;
        com[1] = 0x40; // こちらはデフォルト設定のまま
        spi.transfer(com,2);
        // 設定結果を読み取るが、必要ではない
        com[0] = (byte)0x8c;
        com[1] = 0x00;
        spi.transfer(com,2);
        System.out.print("設定結果: "+String.format("0x%x ", com[1]));
        com[0] = (byte)0x8d;
        com[1] = 0x00;
        spi.transfer(com,2);
        System.out.println(String.format("0x%x ", com[1]));
        // センサーデータを10ミリ秒間隔で、500個取得する
        for (int i = 0; i <= 500; i++) {
            double[] sdata = sensorValue();
            // 取得データを書き出す
            System.out.println(" " + sdata[0] + " " + sdata[1] + " " + sdata[2]);
            try {
                Thread.sleep(10);
            } catch (InterruptedException ex) {
                Logger.getLogger(SpiTest.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
        spi.closeSpi();
    }
  static public double[] sensorValue(){
        // デバイスからデータ取得
        double [] sense = new double[3];
        byte [][] spi_buff = new byte[3][3];    //送受信用バッファ
        int xregH, xregL, xout;
        int yregH, yregL, yout;
        int zregH, zregL, zout;
        double xac, yac, zac;
        for (int i = 0; i < 3; i++) {
            // 軸アドレス
            spi_buff[i][0] = (byte)(0x80 + 2 * i);
            // 結果受け取りバッファ
            spi_buff[i][1] = 0;
            spi_buff[i][2] = 0;
            spi.transfer(spi_buff[i],3);
        }
        xregH = (spi_buff[0][1] & 0xff);
        xregL = (spi_buff[0][2] & 0xff);
        xout = xregH << 4 | xregL >> 4;
        yregH = (spi_buff[1][1] & 0xff);
        yregL = (spi_buff[1][2] & 0xff);
        yout = yregH << 4 | yregL >> 4;
        zregH = (spi_buff[2][1] & 0xff);
        zregL = (spi_buff[2][2] & 0xff);
        zout = zregH << 4 | zregL >> 4;
        xac = (double) (xout - 2048) / (double) 819;
        yac = (double) (yout - 2048) / (double) 819;
        zac = (double) (zout - 2048) / (double) 819;
        //
        sense[0] = xac;
        sense[1] = yac;
        sense[2] = zac;
        return sense;
    }
}

WiringPiを使わなければ、SPI1は動く

WiringPiのチャンネルをSpi0とSpi1のチャンネルのことだと思っていたが、Slaveポート番号のようだ。ああ、全く。だから、PythonのSpiライブラリを使うと、Spi1が動く。Spi1につけた、加速度センサーKXSD9をちゃんと認識するのだ(以下のPiscopeの画像参照)。どうする?Javaを捨てるわけにもいかんし・・・・・・

Javaで、Spiのデバイスを直接開いて、制御するしかないかもしれない。

40年ぶりのオシロスコープ、SPI1の有効化問題

RaspberryPI3(以下RPI)のSPI通信で、加速度センサーを5個ぶら下げたいのだが、うまくいかないのでここ2、3日呻吟している。
そう言うことで、RPIのgpioピンから出ているパルスをどうしても確かめたくて、前から、必要だと思っていたオシロスコープを秋月電子で買ってきた。SDS5032Eだ。Amazonのそれと値段がほぼ同じなので、秋月電子でもでいいだろうと直接買ったわけだ。
オシロスコープは40年ぶりである。名古屋大学の工学部(電気工学科、内容は情報工学。というのも当時は情報工学科がなかった)の学生だったとき触って以来だった。当時の機械は、数百万円したのではないかと思う。今日、買ってきたのは、3万1千円だった。それでも高いが、性能はこちらの方がいいのだろう。当時はアナログで、それも良かった。
オシロスコープの使い方など、とっくの昔に忘れてしまったが、youtubeの動画をいくつか見たらすぐに勘を取り戻した。昔やっていたから、買ってくればなんとか使えるようになると言う自信があった。大学の教育も、こう言う意味で、役に立つ。ただ、当時は、真面目に勉強しない、半ば落ちこぼれていた劣等生だったが(笑)実験・実習は必修だったので、サボれなかった。
SPI0のSCLKピンを調べるとちゃんとクロックパルスが出ていた。
一方、SPI1のクロックは、40番ピンなのだが、信号が出ていない(泣)。
事前の設定はちゃんとやっている。/boot/config.txt に、次のような設定を書き加えて、rebootしている。
# SPI1 activated
dtoverlay=pi3-disable-bt
dtoverlay=spi1-3cs
bluetoothを無効にしているし、spi1の有効化を指示している。結果的に、
$ ls -l /dev/sp*
crw-rw---- 1 root spi 153, 3 6月 18 20:55 /dev/spidev0.0
crw-rw---- 1 root spi 153, 4 6月 18 20:55 /dev/spidev0.1
crw-rw---- 1 root spi 153, 2 6月 18 20:55 /dev/spidev1.0
crw-rw---- 1 root spi 153, 1 6月 18 20:55 /dev/spidev1.1
crw-rw---- 1 root spi 153, 0 6月 18 20:55 /dev/spidev1.2
で、デバイスの有効化は確認できている。
Spi1のMOSIピンは、38番ピン、MISOピンは35番ピン、両者を直結して、
pythonで、
----------------------
>>> import spidev
>>> spi = spidev.SpiDev()
>>> spi.open(0, 0)
>>> spi.max_speed_hz = 1000000
>>> spi.xfer([0x02])
[0]
>>> spi.xfer([0x1])
[0]
>>> spi.open(1, 0)
>>> spi.max_speed_hz = 1000000
>>> spi.xfer([0x1])
[1]
>>> spi.xfer([0x2])
[2]
--------------------------
と言うようなことをやった。後半部分は、SPI1に関する操作だ、ちゃんと値を返している。
SPIというのは、基本、玉突きデータ送信で、MOSIで、マスター(ここでは、RPI)からデータを送り出すと、MISOでスレーブ(末端デバイス)から送り出されてきたデータを受け取るという仕組みである。したがって直結すると、デバイスはないが、マスター自身から送り出されたものをマスター自身が受け取るということになる。上記では、それをやっているわけで、前半のSPI0は、直結線がないので、何も受け取らないが、後半のSPI1の方は、直結しているので、ちゃんと送り出したデータをそのまま受け取っている。
このデータの送受信をSCLKから出るクロックパルスに基づいて同期化させているのだが、そのパルスが出ていなかったら、うまくいくわけがない。
そうしてだろう???

圧力センサーの装着とデータ


Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_dfa7cc7dcb24dcc08921d96fa0a23e9b.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_dfa7cc7dcb24dcc08921d96fa0a23e9b.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_dcd91e63b5cfbea0e04e9269466ff3b9.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_dcd91e63b5cfbea0e04e9269466ff3b9.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_098d6f3c14c170fc07b67bdabfdfc304.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_098d6f3c14c170fc07b67bdabfdfc304.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_e1e1d3d40573127e9ee0480caf1283d6.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_e1e1d3d40573127e9ee0480caf1283d6.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_54bedf895d7c6608b88041e9c33d1a3f.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_54bedf895d7c6608b88041e9c33d1a3f.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_ee209b85f6fd73430edc5ba414ddfff7.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_ee209b85f6fd73430edc5ba414ddfff7.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_b687ac7bf0999ace0680defac4be196e.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_b687ac7bf0999ace0680defac4be196e.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

Warning: fopen(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_ec69cdd8451f3fb2f240849c33ba32f0.gif): failed to open stream: Permission denied in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 123

Warning: fputs() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 124

Warning: fclose() expects parameter 1 to be resource, bool given in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 126

Warning: getimagesize(/data/htdocs/ibot/wpibot//wp-content/plugins/latex/cache/tex_ec69cdd8451f3fb2f240849c33ba32f0.gif): failed to open stream: No such file or directory in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 129

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 47

Warning: preg_replace(): The /e modifier is no longer supported, use preg_replace_callback instead in /data/htdocs/ibot/wpibot/wp-content/plugins/latex/latex.php on line 49

片足に四個、両足で八個の圧力センサー(FSR402)を足の裏に装着した。生成されるデータは、抵抗値でそれを電圧値に変換して、ADコンバーターMCP3208でデジタルデータにして、ロボット上のRaspberryPIで処理・出力させてみる。
人間にとっても、足の裏の微妙な圧力感覚はすごく大事だ。靴下と足の間に小さな異物があっても、気になって仕方がない。昔、経済学者の間で、博士の学位を取ることを「足の裏の米粒」と表現していたことがある。「価値はないが、取らないと気になって仕方がない」ものというわけだ。
写真のように、ロボットの足の裏とセンサーの間にワッシャーを噛ませた。そうしないと、平たいものに平たいものつけても圧力が出ない。ロボットが、配線のお化けのようになってきた。
足の裏に磁石付き絆創膏を張ったような感じだ。毎秒10個のセンサーデータを10秒間とったデータは以下のようなものだ。

縦軸の出力値は以下のように変換している。
仕様書によれば、ADコンバーターMCP3208の出力値は


で出力される。4096は、12ビットで出力されているということだ。前の記事にもあるように、は、3.3Vである。
一方、記事の図にあるように、FSR402には、5.1KΩ(5100Ω)の抵抗をかましている。学校で習ったようにを応用する。
流れる電流は、FSRの抵抗値をとすると、FSRの両端の電圧


である。
これを変形すると、MCP3208のデータ出力をとして、


となる。これがΩの抵抗値であるから、それをKΩに変換したものが、上記の縦軸である。
この抵抗値までは、ロバスト(頑健性)な値だが、では実際これがどれだけの圧力かというのは、いろいろな誤差もある値になる。仕様書によればおよそ以下のような関係があるという。
縦軸が抵抗値(KΩ)で横軸が重力加速度で表した力だと思う。その力が幾つだからというのは、あまり関係がない。先の図では、だいたい1kΩから10kΩの範囲の値を示しているので、その間の圧力との関係がおよそ上記のようなものだということを知っていれば良いと思う。縦軸は対数目盛になっているので、原点に対する凸性は、実際の値ではもっと厳しくなっている。
そこまでわかったところで、先の図の8個のセンサーの出力を見てみよう。上に行くほど圧力が弱くなっているので、足が浮き加減になると10に近い値まで跳ね上がっている。
じゃあ、全体として何が言えるの?と聞かれても、私には難しい。そこで、人工知能にこれを捉えてもらって、ロボット自身の姿勢制御に使いなさいとうことなのだ。さらに、加速度センサーも追加するので、そういう、ロボットの感覚情報を、プログラマブルに、外側の人間がああだこうだというのは、意味がない。
さしあたって、加速度センサーを、足のむこうずねと、足のひらに4個追加してみよう。センサーをフル装備させるのだ。

全てJAVAでいけることになった

サーボの制御も、センサーの制御も全てJAVAでできるようになった。脳に当たるディープラーニングするニューラルネットもJAVAで動かすので、JAVAで閉じられる。
さしあたって、ロボットの制御プログラムをC++から、JAVAに書き換えよう。

加速度センサーデータのJavaによる取得(プログラム付)

加速度センサーKXSD9のデータをJavaで取ろうとしたら、問題にはまって、なかなか解決しなかったがようやく原因がわかって綺麗なデータを取れるようになった。
初めは、なんか、データが飛び飛びになってうまくいかなかったのだ。こんな感じである。さっぱりダメだった。

先の投稿にも書いたように、電圧のせいかと思って、電源からDCコンバーターM78AR033-0.5を入れてコンデンサーつけたりして3.3Vを独立に確保したが、これも結局問題解決にならず。元のC++のプログラムだと綺麗にデータが取れるので、配線の間違いでもセンサーの問題でもRaspberryPIの問題でもない。
結局わかった。問題は、WiringPi経由でデータを取得するとき、C++の場合は、unsigned charのバッファで取得するのだが、Javaの場合はbyte配列を用いる。それが当然だと思って、そうしているだけで何かに暗示されたわけではない。
私はてっきり、byteはunsigned charと同じで整数変数にキャストして入れても負数にはならないだろうと思いこんでいた。つまり、0-255の範囲の値を出すだろうと思っていたのだが、実は、-127から+127の値を出力して、整数変数に自動キャストで入れると突然負の値が出てきてしまうのだ。
センサーデータは、12ビットで出てくるので、C++の場合、一つの軸(例えばX軸)のデータは、以下のように変換する。
xregH = spi_buff[0][1];
xregL = spi_buff[0][2];
xout = xregH << 4 | xregL >> 4;
spi_buffはunsigned charのバッファで、xregH、xregLは上位ビットの整数値、下位ビットの整数値を取り出して、上位を4ビット左にシフトさせ、下位ビットを4ビット右にシフトさせて論理和を取れば12ビットの値が取れる。そこに負が出てくるなど想定しなくていい。
ところが、同じことをunsigned charの代わりにJAVAのbyteでやると簡単に負になってしまって、予期しない値が取れてしまうというわけだ。結局、整数に変換しするときに上位ビットをマスクするという操作が必要だった。
xregH = (spi_buff[0][1] & 0xff);
xregL = (spi_buff[0][2] & 0xff);
xout = xregH << 4 | xregL >> 4;
こうすればうまくいくことがわかったのである。
ちょんちょん!!
JAVAのプログラムは次のようになる。Pi4Jのライブラリが必要になることは、前にこちらに書いたのでそれを参考にしてほしい。

package aicoro;
import com.pi4j.wiringpi.Gpio;
import com.pi4j.wiringpi.Spi;
/**
 *
 * @author Toyoaki WASHIDA
 */
public class Accelerometer {
    int speed = 1000000; //通信速度(Hz)
    int SPI_CHANNEL = Spi.CHANNEL_1;
    int SS_PORT = 7;
    Accelerometer() throws InterruptedException{
        // setup SPI for communication
        System.out.println("SPIチャンネル初期化します");
        int fd = Spi.wiringPiSPISetup(SPI_CHANNEL, speed);
        if (fd <= -1) { System.out.println(" ==>> SPI SETUP FAILED");
            return;
        }
        System.out.println("PIO初期化します");
        if (Gpio.wiringPiSetup() == -1) {
            System.out.println(" ==>> GPIO SETUP FAILED");
            return;
        }
        Gpio.pinMode (SS_PORT, Gpio.OUTPUT);
        //SS信号初期化
        Gpio.digitalWrite(SS_PORT, 1);
        Thread.sleep(0, 130);
        byte com[] = new byte[2];
        com[0] = 0x0c;  // address byte
        com[1] = (byte) 0xe3;  // register byte
        // 設定を(+/-2g 819 counts/g)にして、小さい範囲を細かく見ている
        Gpio.digitalWrite(SS_PORT, 0);
        int err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー(1)");
            return;
        }
        Thread.sleep(0, 130);
        com[0] = 0x0d;
        com[1] = 0x40; // こちらはデフォルト設定のまま
        Gpio.digitalWrite(SS_PORT, 0);
        err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー(2)");
        }
        /////////////////設定状況を読み取る/////////////////////////////
        Thread.sleep(0, 130);
        com[0] = (byte) 0x8c;
        com[1] = 0x00; // こちらはデフォルト設定のまま
        Gpio.digitalWrite(SS_PORT, 0);
        err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー(2)");
        }
        System.out.println("CTL_REGCの値 = "+String.format("0x%02x",com[1] ));
        Thread.sleep(0, 130);
        com[0] = (byte) 0x8d;
        com[1] = 0x00; // こちらはデフォルト設定のまま
        Gpio.digitalWrite(SS_PORT, 0);
        err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー(2)");
        }
        System.out.println("CTL_REGBの値 = "+String.format("0x%02x",com[1] ));
    }
    double[] sensorValue() throws InterruptedException {
        // デバイスからデータ取得
        double [] sense = new double[3];
        byte [][] spi_buff = new byte[3][3];    //送受信用バッファ
        int xregH, xregL, xout;
        int yregH, yregL, yout;
        int zregH, zregL, zout;
        double xac, yac, zac;
        for (int i = 0; i < 3; i++) {
            spi_buff[i][0] = (byte)(0x80 + 2 * i);
            spi_buff[i][1] = 0;
            spi_buff[i][2] = 0;
            Gpio.digitalWrite(SS_PORT, 0);
            int err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, spi_buff[i], 3); //データ送受信
            Gpio.digitalWrite(SS_PORT, 1);
            Thread.sleep(1, 130);
            if (err == -1) {
                System.out.println("sensorValue: 書き込みエラー(1)");
                break;
            }
            //System.out.println("データを受け取りました ret = "+err);
        }
        xregH = (spi_buff[0][1] & 0xff);
        xregL = (spi_buff[0][2] & 0xff);
        //System.out.println("xregH = "+xregH+" xregL = "+xregL); //(short) (valueByte & 0xFF);
        xout = xregH << 4 | xregL >> 4;
        yregH = (spi_buff[1][1] & 0xff);
        yregL = (spi_buff[1][2] & 0xff);
        yout = yregH << 4 | yregL >> 4;
        zregH = (spi_buff[2][1] & 0xff);
        zregL = (spi_buff[2][2] & 0xff);
        zout = zregH << 4 | zregL >> 4;
        xac = (double) (xout - 2048) / (double) 819;
        yac = (double) (yout - 2048) / (double) 819;
        zac = (double) (zout - 2048) / (double) 819;
        //
        sense[0] = xac;
        sense[1] = yac;
        sense[2] = zac;
        return sense;
    }
}

加速度センサーのデータ落とし

JAVAでSPI経由で加速度センサーがなんとか制御できそうになってきたのに、加速度センサーがデータを落とすときがある。JAVAプログラムの原因ではなく、加速度センサーは複数で確認した、圧力センサーは同じSPIポートで正常なので、3.3Vの電圧不足なのではないかと、疑っている。

SPIの配線

シリアル接続が、サーボ群の制御用にはI2Cを使い、圧力センサーと3D加速度センサーと両方でSPIをパラレルに使うようになって、RaspberryPI上の配線が異様に複雑になってきた。
SPIをパラレルに幾つも使うために、クロック(SCLK)と入力(MOSI)及び出力(MISO)はバス方式で使うようにRaspberryPIの上のボードにコネクタをつけた。コネクタのどのピンがどれに対応しているかを忘れてしまいそうだから、上に画像をつけた。要するに、右からクロック(SCLK)、入力(MOSI)、出力(MISO)のピンとなっている。
なお、GPIOのCE0ピンを圧力センサ用に、CE1ピンを加速度センサ用に使っている。

I2Cを経由して、JAVAでサーボモータを制御する

この間、ディープラーニングは圧倒的にC++よりもJAVAが良いことがわかって、全てをロボットの制御システムを全てJAVAに変えようとしている。
センサー系は、SPIで制御するのだが、これはPi4Jプロジェクトのライブラリでなんとかなりそうなことはわかった。
サーボモーターは、I2C経由で制御する。これも、Pi4Jのライブラリを使えばなんとかなりそうだということで、昨夜から格闘して、ようやくなんとかなってきた。いつものように記録しておく。
pi4jには、サーボモーター制御のサンプルコードは、
PCA9685GpioServoExample.java
PCA9685GpioExample.java
の二つがある。Servoという言葉が入っている初めのものを使おうとしたが、結局、なぜかサーボが異常に熱くなってしまい、動かないので、2番目のを使った。
初めは、パルスの作り方などを細かく変えなくちゃいけないんじゃないかと思って色々いじったが、結局、ほとんど素のままで使えることがわかった。ありがたい。使うところだけ抜き出して、インターフェイスの部分の関数を付け加えると次のようになる。

package aicoro;
import com.pi4j.gpio.extension.pca.PCA9685GpioProvider;
import com.pi4j.gpio.extension.pca.PCA9685Pin;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinPwmOutput;
import com.pi4j.io.gpio.Pin;
import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CFactory;
import java.io.IOException;
import java.math.BigDecimal;
/**
 * Pi4JのPCA9685GpioServoExample.javaを参照したもの
 */
public class Servo {
    // MG996R用に調整した
    private static final int SERVO_DURATION_MIN = 770;  // 1670/2=835
    private static final int SERVO_DURATION_NEUTRAL = 1570;
    private static final int SERVO_DURATION_MAX = 2370;
    final PCA9685GpioProvider provider;
    Servo() throws I2CFactory.UnsupportedBusNumberException, IOException{
        System.out.println("PCA9685を初期化します");
        //PCA9685GpioServoExample.javaの値をそのまま使っている
        BigDecimal frequency = new BigDecimal("48.828");
        BigDecimal frequencyCorrectionFactor = new BigDecimal("1.0578");
        // Create custom PCA9685 GPIO provider
        I2CBus bus = I2CFactory.getInstance(I2CBus.BUS_1);
        provider = new PCA9685GpioProvider(bus, 0x40, frequency, frequencyCorrectionFactor);
        // Define outputs in use for this example
        GpioPinPwmOutput[] myOutputs = provisionPwmOutputs(provider);
        // Reset outputs
        provider.reset();
    }
    void setAngle(int ch, double degree) {
        if (degree < -90 || degree > 90) {
            System.out.println("setAngle() range err give degree = " + degree);
            return;
        }
        int pulseWidth = SERVO_DURATION_NEUTRAL + (int) (SERVO_DURATION_NEUTRAL * (degree / 90.0));
        //cout << "setAngle() pulseWidth = " << pulseWidth << endl;
        Pin pin = PCA9685Pin.ALL[ch];
        provider.setPwm(pin,pulseWidth);
    }
    private static GpioPinPwmOutput[] provisionPwmOutputs(final PCA9685GpioProvider gpioProvider) {
        // サーボモータの名前を与える、どの関節に関わっているかを明確にする意味がある
        GpioController gpio = GpioFactory.getInstance();
        GpioPinPwmOutput myOutputs[] = {
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_00, "Pulse 00"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_01, "Pulse 01"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_02, "Pulse 02"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_03, "Pulse 03"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_04, "Pulse 04"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_05, "Pulse 05"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_06, "Pulse 06"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_07, "Pulse 07"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_08, "Pulse 08"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_09, "Pulse 09"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_10, "Always ON"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_11, "Always OFF"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_12, "Servo pulse MIN"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_13, "Servo pulse NEUTRAL"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_14, "Servo pulse MAX"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_15, "not used")};
        return myOutputs;
    }
}

必要ないかもしれないが、センターの位置だけ少し変えている。

void setAngle(int ch, double degree)

関数に、サーボモーターの番号と、角度を与えれば良い。javaのmain関数は、別に与えること。その中で、このクラスをインスタンス化して、上の関数を呼べば良い。
不要かもしれないが、動画をつけておく。