カテゴリー: センサー

Lidar lite3 の電源問題

先の記事で、lidar lite3の電源をraspi本体のピンから取らないとダメだったと書いたが。HAL2で試したところ、通常の電源で問題なく動いた。電圧が一定程度ないといけないのかもしれない。

衝突を回避しながらランダムウォーク

これはあまり舞台上で必要な機能ではないのですが、Lidar Lite V3を使って、衝突を回避しながら、壁沿いをランダムに歩くシミュレーションです。

残されたログはこれである。測定した距離に障害物があったら、それをMap情報にしたほうがいいよね。今は、全然地図を使っていないので、無駄な動きがある。

超音波距離センサーの問題

Lidar Lite V3の赤外線距離センサーを前方につけて、自己位置を同定したり、それをもとに、位置調整をしたりはできるようになった。

そこで、残りの三方についている超音波距離センサーでも、できるようにプログラムはくんだ。ただ、センサーがすぐに働かない。C++で書いたセンサーのプログラムを、JAVAのコアシステムから呼び出すようにしてあるのだが、少し、C++の方のプログラムを動かしてからじゃないと、距離が取れない。以下の写真の、右上のように直接何度か動かすと、左下のようにコアシステムから距離が取れて、どこまで正しいか検証はしていないが、自己位置を推定している。

Lidar Lite V3の時のような、電源問題ではないかと思った。ので、Lidar Lite V3と同じように、GPIOから電源をとってテストしようかと思ったが、そもそも、超音波センサーをあまり信頼していないのに、いじっても仕方がないなと思い。これはここで凍結しておこうと思う。

絶対使わないならば、センサーそのものを外しても良いのだが、まあ、その選択肢も睨みながらの保留だ。

自己位置認識とターゲットへの移動

Lidar Lite v3で測定された距離センサーデータ、方位センサーで認識した絶対ロボット角度、さらに舞台角度情報をもとにロボットが自己位置を認識し、目標指定位置へ移動するテスト。

まずその位置を舞台上の座標で、推定する。その後、指定位置方向へ向きを変えて（動画の場合、ほとんど変わっていない）移動するテスト。うまく動いているかどうかはこれからチェック。

Lidar Lite v3の電源問題

予備のraspberryPI3で、Lidar lite v3が軽く動いたので、ロボット本体でも動くだろうと組み込んだら、I2Cのデバイスにすら認識しなくなった。

ほぼ、一日、すったもんだしていた。I2C上のコンパスセンサーなどは正しく認識する。同じバス上に置いた、しかも同じ電源を利用しているのに、i2cdetectでアドレスが引っかかってこない。どこに問題があるか、色々試してみたが、結局、電源の問題だった。デバイス電源は、raspberrypiに供給している同じ電源からとっていたのだが、こうする限り、デバイスを認識しない。コンパスセンサーはこれで良かったのだ。

GPIOピンの電源から5Vを引いてきて、デバイスに与えたら認識した。なんで、と思うが、微妙な問題があるのだろう。raspberrypiから、ロボットのヘッドへつなげるコードがまた二本増えてしまった（泣）

レーザー距離センサーLidar Lite v3を動かすjavaプログラム

RobotShopで16,000円以上もする距離センサーLidar Lite v3を購入した。

I2Sで動かそうとしたのだが、ネット上のプログラムでは動かなかった。理由はわからない。そこで、pi4jライブラリを使ってjavaで動かすプログラムを作成したら、すんなり動いてくれた。

ここに公開しておく。（こちらのpythonプログラムを参考にさせていただいた）

import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
 
/**
 *
 * @author washida
 */
public class LidarLiteV3 {
    public static final int LIDAR_LITE_ADDR = 0x62; // address pin not connected (FLOATING)
    public static final byte ACQ_COMMAND = (byte) 0x00;
    public static final byte STATUS = (byte) 0x01;
    public static final byte FULL_DELAY_HIGH = (byte) 0x0f;
    public static final byte FULL_DELAY_LOW = (byte) 0x10;
 
    /**
     * @param args
     * @throws com.pi4j.io.i2c.I2CFactory.UnsupportedBusNumberException
     * @throws java.io.IOException
     */
    public static void main(String[] args) throws I2CFactory.UnsupportedBusNumberException, IOException  {
        I2CBus i2c = I2CFactory.getInstance(I2CBus.BUS_1);
        I2CDevice device = i2c.getDevice(LIDAR_LITE_ADDR);
        while (true) {
            int response = device.read(0x04);
            device.write(ACQ_COMMAND, (byte) response);
            try {
                Thread.sleep(500);
            } catch (InterruptedException ex) {
                Logger.getLogger(LidarLiteV3.class.getName()).log(Level.SEVERE, null, ex);
            }
            int value = device.read(STATUS);
            while ((value &amp; 0x01) == 1) {
                value = device.read(STATUS);
            }
            int high = device.read(FULL_DELAY_HIGH);
            int low = device.read(FULL_DELAY_LOW);
            int val = (high &lt;&lt; 8) + low;
            int dist = val;
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                Logger.getLogger(LidarLiteV3.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
}

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

４つの距離センサは、Cで制御し、4つのモータのエンコーダーからの回転数取得はjavaで制御しなければならない事情を先に書いた。そこで、２つのMCP23S17を使って、それぞれを距離センサとエンコーダ処理用にする戦略をとることにした。
RaspberryPi の一つのSPI０チャンネルに、上記の二つのMCP23S17をバスでぶら下げることにする。デバイスの識別は、A0,A1,A2端子を１、０で区別すればいい（８個区別出来る）。基本的に、０番と１番のデバイスにする。
距離センサは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をデバイス０番でセットしておく
使い方：
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;
}
// セットアップ関数の呼び出しは、最初の１回だけである
//　何度も呼び出してはならない
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();
    }
}

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

月	火	水	木	金	土	日
« 9月
1	2	3	4	5	6	7
8	9	10	11	12	13	14
15	16	17	18	19	20	21
22	23	24	25	26	27	28
29	30