日別: 2018年6月17日

レーザー距離センサー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);
            }
        }
    }
}