Cで書いた距離センサモジュールをjavaから呼び出す

4つの距離センサは、Cで制御し、4つのモータのエンコーダーからの回転数取得はjavaで制御しなければならない事情を先に書いた。そこで、2つのMCP23S17を使って、それぞれを距離センサとエンコーダ処理用にする戦略をとることにした。
RaspberryPi の一つのSPI0チャンネルに、上記の二つのMCP23S17をバスでぶら下げることにする。デバイスの識別は、A0,A1,A2端子を1、0で区別すればいい(8個区別出来る)。基本的に、0番と1番のデバイスにする。
距離センサはCプログラムで制御するのだが、ロボットの方は、javaで制御システムが書かれているので、センサのCプログラムは、ライブラリ化(libdistance.so)して、jna経由で、javaから直接そのライブラリを使うようにすればいい。これはすでにやったことがあるので、簡単にできると思ったが、結局、昨日の夜から今日の夜まで、かかった(途中、大学に行って学生の修論指導はしたが)!
第一に手こずった問題は、libdistance.soを作成して、javaのjnaを使ったプログラムを書いて、lib distance.soをLD_LIBRARY_PATHの環境変数で与えられる場所において、動かしてみたのだが、どうやっても、
「java.lang.UnsatisfiedLinkError: /tmp/jna-3577/jna760224136558438898.tmp: /tmp/jna-3577/jna760224136558438898.tmp: 共有オブジェクトファイルを開けません: そのようなファイルやディレクトリはありません」
というエラーが出る。色々やって、かなり長時間格闘して、結論的には、最新のjna.jar (4.5.1)とraspberrypiの何らかの相性が悪いという判断をした。同じ状況で、linux(ubuntu16.04)では、ちゃんと動くのだから。
これは間違っているかもしれない。が、私の能力の範囲での結論はそうなのだ。だから、去年ちゃんとraspberrypi上で動いたjna.jarを持ってきて、それでセットしたらちゃんと動いた。あ〜あ。
もう一つ、こちらは大したことはないのだが、wiringPiのライブラリもちゃんとくっつけて、libdistance.soを作成しないと、wiringPiの関数が見つからないと怒られる!
プログラムをいかに掲載する。まず、Cライブラリのプログラム distance.cは以下のようになる。

/*
距離センサーライブラリ
MCP23S17をデバイス0番でセットしておく
使い方:
getdistance('センサー番号:整数 0-3')
*/
#include <stdio.h>
#include <wiringPi.h>
#include <mcp23s17.h>
#include <sys/time.h>
#include <unistd.h>
// 64より大きな数字、適当 wiringpiの要請
#define    BASE       123
int trigPin, echoPin;
int pulseIn(int pin, int level, int timeout){
  struct timeval tn, t0, t1;
  long micros;
  gettimeofday(&amp;t0, NULL);
  micros = 0;
  while (digitalRead(pin) != level){
      gettimeofday(&amp;tn, NULL);
      if (tn.tv_sec &gt; t0.tv_sec) micros = 1000000L; else micros = 0;
      micros += (tn.tv_usec - t0.tv_usec);
      if (micros &gt; timeout) return 0;
    }
  gettimeofday(&amp;t1, NULL);
  while (digitalRead(pin) == level){
      gettimeofday(&amp;tn, NULL);
      if (tn.tv_sec &gt; t0.tv_sec) micros = 1000000L; else micros = 0;
      micros = micros + (tn.tv_usec - t0.tv_usec);
      if (micros &gt; timeout) return 0;
    }
  if (tn.tv_sec &gt; t1.tv_sec) micros = 1000000L; else micros = 0;
  micros = micros + (tn.tv_usec - t1.tv_usec);
  return micros;
}
// セットアップ関数の呼び出しは、最初の1回だけである
// 何度も呼び出してはならない
void setup(int pin){ // pin -&gt; 0-3
	trigPin = BASE+pin+8; // B0-B3
	echoPin = BASE+pin;   // A0-A3
    wiringPiSetup () ;
    mcp23s17Setup (BASE, 0, 0) ;
    pinMode (trigPin, OUTPUT) ;
    pinMode         (echoPin, INPUT) ;
    pullUpDnControl (echoPin, PUD_UP) ;
}
int getdistance(){
    long duration, distance;
	// HIGHが10μs継続するパルスを出す
    digitalWrite(trigPin, LOW);  // Added this line
    delayMicroseconds(2); // Added this line
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10); // Added this line
	digitalWrite(trigPin, LOW);
	// その結果出力された超音波を受け取る
	// その時間差から距離を測る
    duration = pulseIn(echoPin, HIGH, 1000000);
    //printf("duration=%d ",duration);
    distance = (duration/2) / 29.1;
    //printf("距離 = %d cm\n",distance);
    //sleep(1);
	return (int)distance;
}

これをコンパイルするためには次のようにする。
gcc -fPIC -shared -o libdistance.so distance.c -lwiringPi
最後に、wiringPiのライブラリをくっつけているところ大事。というのも、これをつけなくても、コンパイル自身は難なく通ってしまうから、実行時に叱られる。
export LD_LIBRARY_PATH=../XXX :$LD_LIBRARY_PATH
sudo ldconfig
とかする。../XXXはlibdistance.soをおく場所である。あるいは、
/etc/ld.so.conf.d
の設定ファイルに、そのパスを書いておき、ldconfigを実行するとかする。

import com.sun.jna.Library;
import com.sun.jna.Native;
import java.util.logging.Level;
import java.util.logging.Logger;
// ここの部分がlibdistance.soと接続するおまじない
interface DistanceLib extends Library {
    DistanceLib INSTANCE = (DistanceLib) Native.loadLibrary("distance", DistanceLib.class);
    // 使うべき関数を宣言する
    int getdistance() ;
    void setup(int pin);
}
/**
 *
 * @author washida
 */
public class Distance {
    DistanceLib dlib = DistanceLib.INSTANCE;
    void show(){
        dlib.setup(0);
        for(int i=0;i&lt;100;i++){
            int dis = dlib.getdistance();
            System.out.println("No."+ i +": 距離 = "+dis+"cm");
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                Logger.getLogger(Distance.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
    /**
     * @param args the command line arguments
     */
    public static void main(String[] args) {
        // TODO code application logic here
        Distance dis = new Distance();
        dis.show();
    }
}

こちらは特に問題はないだろう。

メカナムホイール の装着

ロボットエミリーを前後左右斜めに自由に動かそうと、メカナムホイール を装着した。

ただ、最初付け方を間違えた。右2個、左2個買ってきて、その通り右と左につけたら、前進後退以外うまく動かない。よくよく調べてみると、ホイールの向きが間違っていた。前後に逆のものをつけなければならなかったのだ。笑ってしまった。
動画を見てもらえばわかると思う。

TwitterStreamingによるツイートイベントの取得

Twitter Botの前の記事の仕様では、bot側から@aigeininへのツイートを取得しなければならなかった。そのために、お題が投稿されたタイミングがわからないから、15秒おきにツイートを取りに行っていた。これが面倒だった。
そこで、TwitterStreamingAPIを使って、投稿のイベントを取得することにした。
twitter4jのサンプルにちょっとだけ手を加えたものは次のようになる

public static void main(String[] args) throws TwitterException {
    TwitterStream twitterStream = new TwitterStreamFactory().getInstance();
    twitterStream.setOAuthConsumer(consumerKey, consumerSecret);
    twitterStream.setOAuthAccessToken(new AccessToken(accessToken, accessTokenSecret));
    AiGeininBot2 aig = new AiGeininBot2();
    StatusListener listener = new StatusListener() {
        // フィルターをかけたツイートが取れると、このリスナーが呼び出される
        @Override
        public void onStatus(Status status) {
            // ツイート内容がStatusで与えられる
            System.out.println("@" + status.getUser().getScreenName() + " - " + status.getText());
            // statusを与えて、次のメソッドで処理する
            aig.execNazokake(status);
        }
        @Override
        public void onDeletionNotice(StatusDeletionNotice statusDeletionNotice) {
            System.out.println("Got a status deletion notice id:" + statusDeletionNotice.getStatusId());
        }
        @Override
        public void onTrackLimitationNotice(int numberOfLimitedStatuses) {
            System.out.println("Got track limitation notice:" + numberOfLimitedStatuses);
        }
        @Override
        public void onScrubGeo(long userId, long upToStatusId) {
            System.out.println("Got scrub_geo event userId:" + userId + " upToStatusId:" + upToStatusId);
        }
        @Override
        public void onStallWarning(StallWarning warning) {
            System.out.println("Got stall warning:" + warning);
        }
        @Override
        public void onException(Exception ex) {
        }
    };
    twitterStream.addListener(listener);
    // ここで @aigeinin 向けたツイート、リプライだけを取得するためのフィルターを作る
    final String[] TRACK = { "@aigeinin" };
    FilterQuery filter = new FilterQuery();
    filter.track(TRACK);
    // ここでフィルターを組み込む
    twitterStream.filter(filter);
}

RaspberryPIで二つ目のI2CチャンネルをPCA-9685用に動かす

足郎2では、左右の足を別のスレッドで動かし、かつそれぞれ別のI2Cチャンネルを使うことにしている。
RaspberryPIで二つ目のI2Cチャンネルを動かすことについては、
http://robo.genv.sophia.ac.jp/wp_robot/?p=1645
にだいぶ書いておいた。
ただ、これをサーボドライバPCA-9685用に動かすためには、一つ面倒な作業がある。それは、I2Cのデフォルトである1チャンネルは、すでにRaspberryPIのボード内でプルアップされているのであるが、この第二チャンネルは、プルアップされていないので、外部プロアップをしなければならないのである。
ただ、秋月電子のボードには、プルアップ用の抵抗がすでに取り付けられていて、このJ1とJ2を、直結すればSDAとSCLの二つのピンがプルアップされる。

直結する方法に戸惑ったが、ハンダで無理やりすれば良いのだと了解して、そうしている。今の所不都合はない。

ローカル座標のグローバル座標への転換戦略

表題のことがテーマである。
ずっと悩んでいる(笑)
(1)まず、片足に重心がほぼ乗っているかどうかの場合を分ける。
(2)乗っていない場合。乗っていなくても、相対的に重心がかかっている足は確定できる。その相対的支持足には四つのポイントがある。
z軸の値が小さなものが少なくとも二つはあるはずだ。
ロボットの体型が決まっていれば、その点を基準に他の足や脚の座標が全て決められるはずだ。
(※ここで注意しておくと、以後、足のくるぶしから先の部分を足(英語でfoot)といい、その他の英語でlegと言われる部分は脚ということにする)
その中で、z座標が負になるような点があったら、初めの2点の角度を変えて、それらの点がゼロになるように角度を変えなければならない。
逆に逆足が中空に上がってしまった場合は、そこを大地にくっつけるように回転させる必要がある。
(3)重心がその脚だけに乗っている場合は、その脚全体が大地についていることを前提に体型を載せる必要がある。そして、もし、逆足が大地から上がっている場合は、それをそのままにしておいて良い。重心がほとんど乗っていないわけだから。

ロボット座標の再構成(修正)

いざプログラムに書こうと思ったら、先の記事に書いた正面図が、ひざ関節が左右に曲がるというものになっているのに気づいた。足郎2は、ひざは前後にしか曲がらない(人間と同様)ので、修正した。

正面図は、変な格好だが、図は角度の正の向きを確認し、関節位置の座標を確定するためのものだから問題ない。
《正面図》
A点とB点の関係

y_{a}=y_{b}+l_{1}sin(\frac{\pi}{2}-\theta_{3})\\z_{a}=z_{b}-l_{1}cos(\frac{\pi}{2}-\theta_{3})


B点とC点の関係

y_{b}=y_{c}+l_{2}sin(\frac{\pi}{2}-\theta_{3})\\z_{b}=z_{c}-l_{2}cos(\frac{\pi}{2}-\theta_{3})


C点と原点の関係

y_{c}=y_{0}-\frac{l_{3}}{2}\\z_{c}=z_{0}


D点と原点の関係

y_{d}=y_{0}+\frac{l_{3}}{2}\\z_{d}=z_{0}


D点とE点の関係

y_{e}=y_{d}+l_{4}sin(\frac{\pi}{2}-\theta_{4})\\z_{e}=z_{d}-l_{4}cos(\frac{\pi}{2}-\theta_{4})


E点とF点の関係

y_{f}=y_{e}+l_{5}sin(\frac{\pi}{2}-\theta_{4})\\z_{f}=z_{e}-l_{5}cos(\frac{\pi}{2}-\theta_{4})


《側面図》
A点とB点の関係

x_{a}=x_{b}-m_{1}sin(\phi_{2}+\phi_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-m_{1}cos(\phi_{2}+\phi_{3}-\frac{\pi}{2})


B点とC点の関係

x_{b}=x_{c}+m_{2}sin(\frac{\pi}{2}-\phi_{3})\\z_{b}=z_{c}-m_{2}cos(\frac{\pi}{2}-\phi_{3})


C点とD点と原点の関係

x_{c}=x_{d}=x_{0}\\z_{c}=z_{d}=z_{0}


D点とE点の関係

x_{e}=x_{d}-m_{4}sin(\frac{\pi}{2}-\phi_{4})\\z_{e}=z_{d}-m_{4}cos(\frac{\pi}{2}-\phi_{4})


E点とF点の関係

x_{f}=x_{e}-m_{5}sin(\frac{\pi}{2}-\phi_{4}+\phi_{5})\\z_{f}=z_{e}-m_{5}cos(\frac{\pi}{2}-\phi_{4}+\phi_{5})


 

シミュレーションの長さの計算

一つ前の記事で、次に検討すると記述した長さについての関係性をまとめておく。

図において、OGは、足などの実体であり長さはLである。この長さが正面図において見えているのが、OLである。また、側面図において見えているのがOHとなる、それらの長さはlmである。正面図、側面図では、角度θとφが捉えられているとしよう。
このときまず、の位置は同じだから、

mcos\phi=lcos\theta


が成立する。
さらに、

L=\sqrt{m^{2}sin^{2}\phi+l^{2}sin^{2}\theta+m^{2}cos^{2}\phi}


が成立する。これら二つの式より、l,mLと角度の値から求めることができる。

ロボット座標の再構成

先の記事に書いて、シミュレーションで暫定的に動かしてみた座標の設定には、問題があった。

正面図と側面図を同時に書くと上記図のようになる。前回の問題は、ロボットの原点で座標が固定されていなかったことだ。原点の軸においては、回転していなくした。そこで固定して、実際の動きは、それぞれの位置をグローバル座標に変換して表示させるようにすべきだということ。
図はまた、角度の正の向きを定義している。単なる定義であって、定義を変えると途中の式の形は変わるが、結果に変化はない。
原点の座標をO(x_{0},y_{0},xz_{0})としてA,B,C,D,E,Fの各座標を示そう。ただし、\theta_{1},\theta_{6},\phi_{1},\phi_{6}は、座標これらの座標に直接関係せず、ただ、足の向きを規定しているだけなので、ここでは省略する。
《正面図》
A点とB点の関係

y_{a}=y_{b}-l_{1}sin(\theta_{2}+\theta_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-l_{1}cos(\theta_{2}+\theta_{3}-\frac{\pi}{2})


B点とC点の関係

y_{b}=y_{c}+l_{2}sin(\frac{\pi}{2}-\theta_{3})\\z_{b}=z_{c}-l_{2}cos(\frac{\pi}{2}-\theta_{3})


C点と原点の関係

y_{c}=y_{0}-\frac{l_{3}}{2}\\z_{c}=z_{0}


D点と原点の関係

y_{d}=y_{0}+\frac{l_{3}}{2}\\z_{d}=z_{0}


D点とE点の関係

y_{e}=y_{d}+l_{4}sin(\frac{\pi}{2}-\theta_{4})\\z_{e}=z_{d}-l_{4}cos(\frac{\pi}{2}-\theta_{4})


E点とF点の関係

y_{f}=y_{e}-l_{5}sin(\theta_{4}+\theta_{5}-\frac{\pi}{2})\\z_{f}=z_{e}-l_{5}cos(\theta_{4}+\theta_{5}-\frac{\pi}{2})



《側面図》

A点とB点の関係

x_{a}=x_{b}-m_{1}sin(\phi_{2}+\phi_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-m_{1}cos(\phi_{2}+\phi_{3}-\frac{\pi}{2})


B点とC点の関係

x_{b}=x_{c}+m_{2}sin(\frac{\pi}{2}-\phi_{3})\\z_{b}=z_{c}-m_{2}cos(\frac{\pi}{2}-\phi_{3})


C点とD点と原点の関係

x_{c}=x_{d}=x_{0}\\z_{c}=z_{d}=z_{0}


D点とE点の関係

x_{e}=x_{d}-m_{4}sin(\frac{\pi}{2}-\phi_{4})\\z_{e}=z_{d}-m_{4}cos(\frac{\pi}{2}-\phi_{4})


E点とF点の関係

x_{f}=x_{e}-m_{5}sin(\frac{\pi}{2}-\phi_{4}+\phi_{5})\\z_{f}=z_{e}-m_{5}cos(\frac{\pi}{2}-\phi_{4}+\phi_{5})


正面図と側面図のz軸の座標は一致しなければならないので、式的にはlmで調整されることになるだろう。
つまり、実際の足の長さ、L_{1}, L_{2}と与えられた角度から、l_{1}, l_{2}, l_{4}, l_{5}, m_{1}, m_{2}, m_{4} ,m_{5}の全てが計算できるはずだ。次回はそれを考えてみる。

逆振り子型二足歩行2

歩行の形を少し整えた。
足が上がっていないにもかかわらず、歩いているかのような感じになってきている(笑)
これで、折りたためる膝があれば、普通に歩けると思うのだが。
まあ、もう少し、この膝なし足ロボットで試験しよう。

cosmで書いた制御プログラムは次のようなものだ。何か、それらしい感じを読み取ることができるのかもしれない。

#######################################
# ashiro1-10.cosm
# 逆振り子二足歩行
# 2017年7月4日 作成
# 2017年7月6日 ver.2
# 傾けた時に、前後の足を戻す操作を入れる
#######################################
%defspeed normal {
    interval:3
    #steps:25
    steps:15
}
%defspeed fast {
    interval:3
    #steps:25
    steps:10
}
# 40にすると倒れる
%param invpen_angle 35
# 揺れの逆足の開き角度
# 揺れた時に、揺れる方向の逆足が少し開いていないと、
# 近づきすぎて倒れる:逆足の吊り上げ角度にもなっている
# %param open_inv 15
%param open_inv 20
# 釣り上げられた足の閉じる方向への角度
%param close_inv 10
######################################
# 0 バージョンは、前後の足の動きを意識しないもの
# 最初の揺らしに使うと思う
######################################
%defangles right_invpen_0 {
#右に傾ける動きをする
# 逆足は開きを少し大きくしないと縮まって倒れる
    RightUpperRight:$invpen_angle-$open_inv
    RightUpperLeft:-$invpen_angle+$open_inv
    LeftUpperRight:$invpen_angle-$close_inv
    LeftUpperLeft:-$invpen_angle+$close_inv
# 上の動きに合わせて、足も角度をつけて
# 床にぴったりさせようとしているが、ちょっとずれている
    RightLowerRight:$invpen_angle
    RightLowerLeft:-$invpen_angle
    # 逆足は開きを少し大きくしないと縮まって倒れる
    LeftLowerRight:$invpen_angle
    LeftLowerLeft:-$invpen_angle
}
%defangles left_invpen_0 {
#左に傾ける動きをする
# 逆足は開きを少し大きくしないと縮まって倒れる
    RightUpperRight:-$invpen_angle+$close_inv
    RightUpperLeft:$invpen_angle-$close_inv
    LeftUpperRight:-$invpen_angle+$open_inv
    LeftUpperLeft:$invpen_angle-$open_inv
#
    RightLowerRight:-$invpen_angle
    RightLowerLeft:$invpen_angle
    LeftLowerRight:-$invpen_angle
    LeftLowerLeft:$invpen_angle
}
######################################
# 1 バージョンは、前後の足の動きの中で使う
# 前のステップで開いた足を戻す、前後の操作が入る
# 後ろに残っている足は、前足が垂直になるまでに
# さらに追加的に開く
%param excess_open 10
######################################
%defangles right_invpen_1 {
#右に傾ける動きをする
# 逆足は開きを少し大きくしないと縮まって倒れる
    RightUpperRight:$invpen_angle-$open_inv
    RightUpperLeft:-$invpen_angle+$open_inv
    LeftUpperRight:$invpen_angle
    LeftUpperLeft:-$invpen_angle
# 上の動きに合わせて、足も角度をつけて
# 床にぴったりさせようとしているが、ちょっとずれている
    RightLowerRight:$invpen_angle
    RightLowerLeft:-$invpen_angle
    # 逆足は開きを少し大きくしないと縮まって倒れる
    LeftLowerRight:$invpen_angle
    LeftLowerLeft:-$invpen_angle
# 前後に開いた足の処理
# 右足が前、左足が後ろの状態で開いている
# 前になっている右足を、直立状態まで戻す
# 後ろ足は、床に設置したまま、傾きを加える
#
    RightUpperFront:0
    RightUpperBack:0
    LeftUpperFront:-$step_length-$excess_open
    LeftUpperBack:$step_length+$excess_open
#
    RightLowerFront:0
    RightLowerBack:0
    LeftLowerFront:-$step_length-$excess_open
    LeftLowerBack:$step_length+$excess_open
}
%defangles left_invpen_1 {
#左に傾ける動きをする
# 逆足は開きを少し大きくしないと縮まって倒れる
    RightUpperRight:-$invpen_angle
    RightUpperLeft:$invpen_angle
    LeftUpperRight:-$invpen_angle+$open_inv
    LeftUpperLeft:$invpen_angle-$open_inv
#
    RightLowerRight:-$invpen_angle
    RightLowerLeft:$invpen_angle
    LeftLowerRight:-$invpen_angle
    LeftLowerLeft:$invpen_angle
#
    RightUpperFront:$step_length+$excess_open
    RightUpperBack:-$step_length-$excess_open
    LeftUpperFront:0
    LeftUpperBack:0
#
    RightLowerFront:$step_length+$excess_open
    RightLowerBack:-$step_length-$excess_open
    LeftLowerFront:0
    LeftLowerBack:0
}
######################################
# 前後に開く(すでに重心が移ったのちの動作)
# 歩幅
%param  step_length 15
# 出した方の足が着地する時に、後ろに倒れないよう
# 少し前かがみ加減の角度で足先を出す
%param  slouch 10
######################################
%defangles right_forward_1 {
# 右に傾いた時に、左足を前に、右足を後ろにする動作
    RightUpperFront:$step_length
    RightUpperBack:-$step_length
    LeftUpperFront:-$step_length
    LeftUpperBack:$step_length
#
    RightLowerFront:$step_length
    RightLowerBack:-$step_length
    LeftLowerFront:-$step_length+$slouch
    LeftLowerBack:$step_length-$slouch
}
%defangles left_forward_1 {
# 左に傾いた時に、右足を前に、左足を後ろにする動作
    RightUpperFront:-$step_length
    RightUpperBack:$step_length
    LeftUpperFront:$step_length
    LeftUpperBack:-$step_length
#
    RightLowerFront:-$step_length+$slouch
    RightLowerBack:$step_length-$slouch
    LeftLowerFront:$step_length
    LeftLowerBack:-$step_length
}
%defexec walk {
    stand:all
    speed:normal
    # 右に倒す、前後の動作は何もしない
    # 半分の動きしかできず、揺れが小さいので
    setAngle:right_invpen_0
    # 揺れは早く動かす
    # 左に倒す
    speed:fast
    setAngle:left_invpen_0
    # 足を開く
    # 開くときは、normalにする
    speed:normal
    setAngle:left_forward_1
    # 普通の歩行に移る
    exec:walk_sub,3
}
%defexec walk_sub{
    # 逆の動きに対応するバージョン
    # 右に倒す
    speed:fast
    setAngle:right_invpen_1
    # 足を開く
    speed:normal
    setAngle:right_forward_1
    # 左に倒す
    speed:fast
    setAngle:left_invpen_1
    # 足を開く
    speed:normal
    setAngle:left_forward_1
}
%exec walk 1