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

これはあまり舞台上で必要な機能ではないのですが、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 & 0x01) == 1) {
                value = device.read(STATUS);
            }
            int high = device.read(FULL_DELAY_HIGH);
            int low = device.read(FULL_DELAY_LOW);
            int val = (high << 8) + low;
            int dist = val;
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                Logger.getLogger(LidarLiteV3.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
}