カテゴリー: ロボット製作

色々回り道をしていると、記録していないことは忘れてしまう。足郎のコントロールシステムであるAicoの基本的な使い方を忘れないように記録しておこう。現時点のバージョン（ver.3）のものである。使ってない人には、訳がわからないと思うが、備忘録なので無視願います。
１。起動
java -jar Aico.jar
２。起動オプション
（１）ハイフンなしで　二つの数字を空白を挟んで並べると、サーボ番号とその角度とみなされ、即実行される。
（２）-com　の後に  XXXX.cosmというファイルを指定すると、そのファイルは、制御言語cosmで書かれているとみなされて、記載されているプログラムを即実行する
（３）-stand | -relax | -rest のいずれかの後に、サーボグループを指定すると、それらのグループのサーボを起立：緩和：休息の状態にする。グループとしては、all, left, right, leftUpper, leftLower, rightUpper, rightLower が、指定できる。
（４）-help は使い方の表示
３。コマンドモード
起動オプションを何も指定しない場合、および、起動オプションで指定した動作を終了すると自動的にコマンド解釈モードに入る。
aico >>
というプロンプトが表示され、コマンドを受け付ける。
（１）quit, exit, bye のいずれかが入力されるとAicoを終了する
（２）readの後にcosmのプログラムファイル名（パス）を記載するとそれを読み込む。実行はしない。指定されたプログラムがないと、src/resourcesのフォルダいかにあるかどうかもチェックする。
（３）exec コマンド（単に'e' 一文字でもいい）で、読み込まれたプログラムを実行する。その際、exec allで全てを実行し、 exec xxx:yyy&sss:ttt ・・・で、読み込まれたプログラムの個々のコマンドを実行する。引数部分は、%defexec 内に記載する個別コマンドのフォーマットで記載し、複数のコマンドを実行する場合は'&'でつなげる
（４）chparam（cpでも良い）で、読み込まれたプログラムのパラメータの値を変更できる。
chparam expara1:30&expara2:35 のような感じで指定する。expara1,expara2はパラメータ名の例。
（５）chspeed （csでも良い）で、読み込まれたプログラムの実行速度の定義を変更できる。chspeed interval:2&steps:30 のような感じで指定する。必ず、intervalかsteps、あるいは両方指定される必要がある。
（６）angle （aのみでも良い）個別サーボの角度を与える。angle 4 30 のような感じで指定する。この場合、サーボ番号4を角度30に設定する。
（７）inertia 左右、または、前後に関わるサーボだけを弛緩させる inertia rightleft,30 左右の動きに関わるサーボ群の30度弛緩、rightleft の代わりにfrontbackで、前後のサーボの弛緩。
（８）起動オプション（３）と同じことができる。ただし、'-'はいらない。
（９）各種テストの実行。pres：圧力センサーのテスト、accel：加速度センサーのテスト、memtest：メモリーのテスト、monitest：モニターテスト

左右の足のサーボを、それぞれ別スレッドで動かそうと思った。
RaspberryPIはデフォルトでは１個のI2Cチャンネルだが、簡単に二つのチャンネルを有効化できる。/boot/config.txt に、
dtparam=i2c_vc=on
の一行を加え、/boot/cmdline.txtni に、
dwc_otg.lpm_enable=0 console=serial0,115200 console=tty1 root=PARTUUID=27ca714e-02 rootfstype=ext4 elevator=deadline fsck.repair=yes bcm2708.vc_i2c_override=1 rootwait quiet splash plymouth.ignore-serial-consoles
のように、
bcm2708.vc_i2c_override=1
を加えて、リブートする。
そうすると、
pi@ashiro:~ $ ls -l /dev/i2c-*
crw-rw---- 1 root i2c 89, 0 6月 27 17:17 /dev/i2c-0
crw-rw---- 1 root i2c 89, 1 6月 27 17:17 /dev/i2c-1
の二つのデバイスが現れる。すると、ID_SDとID_SCに割り当てられている27,28番ピンをSDA0とSCL0ピンとして使える。0チャンネルが新しく加わる。（プルアップする必要がある）
$ i2cdetect -y 0
とすれば、０チャンネルにぶら下がっているデバイスが認識できる。
この二つのデバイスに、左足と右足のサーボ群をそれぞれスレッドで動かすようにぶら下げたいのだが、足郎１の上版ボードにPCA9685のボードを乗せる余裕がない。諦めた。
足郎２では、最初から乗せるような設計にしたいと思っている。
（http://robo.genv.sophia.ac.jp/wp_robot/?p=1844 参照）

センサーとサーボの状態を脳に記憶させるのだが、フフォーマットを少し考えた。
記憶から、自己学習させる。
一連のサーボの動態、結果としてのセンサーデータから、その動きが、いい結果をもたらすのか、否か、その評価ができるようにする。
センサーが、ひどい動きをすれば、それは忌むべき状態だ。その、嫌な感覚をロボットに教え込む。
ほんとうは立つところから教えたい。

人工知能を組み込んだ小脳の学習データの記録や、成功、失敗の判断状況がすぐわかるようにするために、Ledを取り付けた。

300オームの抵抗をかましたが、大きすぎたかもしれない。Ledなんて、切れるほどに光らせたらいいと思っているのに以外と暗くなってしまった。

全てのセンサー（足裏に片足４個両足８個の圧力センサー、上版とスネ２個及び足２個の計５個の加速度センサー）をロボットに装着、２つのSpiチャンネルをフル稼働してデータが取れるようになった。
加速度センサの膝下に設置した２個は、X軸が縦になっているので１gのあたりでうろついている。異常値が１つあるが、電源がショートしたか飛んだのではないかと思っている。
圧力センサーの不器用な動きがきになるが、ADコンバータの仕様ではないかと思っている。
ロボットは、配線のお化けのようになった。
次は、これらのセンサーとサーボモーター群をロボットの脳である、ニューラルネットワークシステムにつなげる。

この間使っている、Cで書かれたRaspberrypi3用のSpiライブラリ、および、そのライブラリを呼び出して、加速度センサーKXSD9を制御するJAVAプログラムのサンプルをGithubに公開している。
https://github.com/toyowa/libspi

ただし、Spi0チャンネルのスレーブデバイス1番に2個ぶら下げて、Spi1チャンネルのスレーブデバイス０番に3個ぶら下げた。これで、Spi0チャンネルのスレーブデバイス0番に圧力センサ用のADコンバータをぶら下げた時に全部正常に動けば問題ないが、これはまだ試していない。
理想は、SPI1に5個全部ぶら下げることだが、それはできなかった（一つ前の記事に書いた）。
このために、JAVAから呼び出すCのspiライブラリ
libspi.so.2
は、複数のSPIチャンネル、デバイスを呼び出すことができるように改訂した。
今は、用事があるので、ここまでだ。帰ってから、圧力センサのADコンバータの追加をしてみよう。

RaspberryPIのSpiチャンネルの１、スレーブデバイス０にGpioを変えて複数のぶら下げる試み(チャンネル０は圧力センサーようにしている)。4個めまでは動くのだが5個めが動かないという不思議な現象に行き詰まっている。
（出力データ図、手で少し揺らしていた）
この前作成した、Cのライブラリ（libspi.so）も、まあ、正常に動いてJAVAから呼び出せている。Gpioの電流を使いすぎかとも思ったが、一つのセンサーが使う電流は数百μAがせいぜいで、LEDなどとは違う、わずかだ。（Gpio一個の限度が16mA、トータルで50mAくらいという話だ）
なんとかフェイクでも5個めを動かしたい。

ロボットの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という三軸加速度センサーをぶらせげてスレーブデバイス番号０から読み取るサンプルプログラムを最後のところにおいておく。
データはこんな感じで綺麗に取れる。

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）とスレーブデバイスポート番号（チャンネル０の場合は、０か１、チャンネル１の場合は、０、１、２のいずれか）、そしてクロックスピードである（20Mくらいまではいけるのではないかと思うが、やったことはない）。
transfer関数は、バイト配列に入っているデータを送信し、結果を入れるバッファにもなっている。Spi特有のデータの「玉突き送受信」を再現している。（別々にもできたのだが・・・・・・）。例えば、２バイト送信する場合、１バイトめにコマンド、その結果が２バイト目に入れられて戻ってくるという感じである。センサーのデータの場合は、１バイトめに軸アドレス、２バイトと３バイトめには結果の１２ビットデータが入っていたりするわけである。
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のチャンネルをSpi0とSpi1のチャンネルのことだと思っていたが、Slaveポート番号のようだ。ああ、全く。だから、PythonのSpiライブラリを使うと、Spi1が動く。Spi1につけた、加速度センサーKXSD9をちゃんと認識するのだ（以下のPiscopeの画像参照）。どうする？Javaを捨てるわけにもいかんし・・・・・・

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