Raspberry Pi講座 VL53L0Xを使ったレーダー

今回は前回使った距離センサーVL53L0Xを使って、レーダー(Plan Position Indicator scope)を作ってみたいと思います。
以前使用した複数のパーツを使用しますので、さきに『Raspberry Pi講座 PWMコントローラ ( PCA9685 )』と『Raspberry Pi講座 距離センサ(VL53L0X)』を行いそれぞれのパーツの使用方法をマスターしてください。


今回使用するパーツ

配線図


Raspberry Piのセッティング
          Raspberry Pi講座 PWMコントローラ ( PCA9685 )』と『Raspberry Pi講座 距離センサ(VL53L0X)』で行った場合は再度行う必要はありません。


$sudo raspi-config
    『5 Interfacing Options 』⇒『P5 I2C』⇒『はい』
$sudo apt-get update
$sudo apt-get upgrade
$ sudo apt-get dist-upgrade
$sudo apt-get install python-dev and python3-dev
$sudo apt-get -y install python-smbus i2c-tools
$sudo i2cdetect -y 1

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- 29 -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
70: 70 -- -- -- -- -- -- --
        
※PCA9685はI2Cの0x40と0x70、VL53L0Xは0x29というチャンネルに接続されI2Cで通信ができていることが分かります。

また、『Raspberry Pi講座 距離センサ(VL53L0X)』にある以下の項目を行ってください。
  • VL53L0X APIのダウンロード、解凍、Raspberry Piへコピー
  • VL53L0X APIをRaspberry Pi上でコンパイルする
  • VL53L0XをPythonで使用する
  • 自分でのVL53L0Xを使用するPythonプログラム(vl53l0x_test.py)を作成する


サンプルプログラム(31_rader_vl53l0x_sg90_92r_with_pca9685.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import Adafruit_PCA9685     #PCA9685用のモジュールをインポート
import VL53L0X              #VL53L0X(spi通信を行うためのモジュール)のインポート
import time                 #sleepするためにtimeモジュールをインポートする
import math                 #sin, cos, piのためにmathをインポートする


#SG90とSG92RをPCA9685を使ってコントロールするためのクラス
class SG90_92R_Class:
    # mPin : GPIO Number (PWM)
    # mPwm : Pwmコントロール用のインスタンス
    # m_Zero_offset_duty :

    """コンストラクタ"""
    def __init__(self, Channel, ZeroOffset):
        self.mChannel = Channel                 #サーボのチャンネル
        self.m_ZeroOffset = ZeroOffset          #ゼロ点を合わせるためにDutyをオフセットさせる

        #Adafruit_PCA9685の初期化
        self.mPwm = Adafruit_PCA9685.PCA9685(address=0x40) #address:PCA9685のI2C Channel 0x40
        self.mPwm.set_pwm_freq(60)  #本当は50Hzだが60Hzの方がうまくいく

    """位置セット"""
    def SetPos(self,pos):
        #pulse = 150~650 : 0 ~ 180deg
        pulse = (650-150)*pos/180+150+self.m_ZeroOffset
        self.mPwm.set_pwm(self.mChannel, 0, pulse)

    """終了処理"""
    def Cleanup(self):
        #サーボを10degにセットしてから、インプットモードにしておく
        self.SetPos(90)
        time.sleep(1)

"""レーダークラス"""
class Rader_with_VL53L0X:
    """コンストラクタ"""
    def __init__(self,Servo_Ch=0, Servo_ZeroOffset=0,vl53l0x_offset=0, vl53l0x_scale=1, blaket_length=2.5):
        """サーボをコントロールする SG90_92R_Classのインスタンスを作成"""
        self.Servo  = SG90_92R_Class(Channel=Servo_Ch, ZeroOffset=Servo_ZeroOffset)
        """VL53L0Xのインスタンスを作成"""
        self.tof = VL53L0X.VL53L0X(address=0x29)
        #距離の取得を開始する
        self.tof.start_ranging(VL53L0X.VL53L0X_BETTER_ACCURACY_MODE)
        #事前に取得したキャリブレーション結果をもとにゼロ点をオフセットさせる
        self.vl53l0x_offset = vl53l0x_offset
        #事前に取得したキャリブレーション結果をもとにスケーリングする
        self.vl53l0x_scale = vl53l0x_scale
        #ブラケットの長さ
        self.blaket_length = blaket_length

    """サーボの角度をセットし、障害物までの距離を取得する"""
    def GetDistance(self, servo_angle, sleep=0.001):
        self.Servo.SetPos(servo_angle)                              #サーボのアングルをセットする
        time.sleep(sleep)                                           #サーボが安定するまで1ms待つ
        dist = self.tof.get_distance()/float(10)                    #VL53L0Xから距離[cm]を取得する
        dist = dist*self.vl53l0x_scale+self.vl53l0x_offset+self.blaket_length  #事前のキャリブレーション結果をもとに出力を調整する
        return dist

    """サーボの角度をセットし、サーボの軸原点で障害物のx,y座標を取得する"""
    def GetXY(self, servo_angle, sleep=0.001):
        dist = self.GetDistance(servo_angle, sleep)
        x = dist*math.cos(float(servo_angle)/180*math.pi)
        y = dist*math.sin(float(servo_angle)/180*math.pi)
        return (x,y)

    """終了処理"""
    def Cleanup(self):
        self.Servo.Cleanup()

"""メイン関数"""
if __name__ == '__main__':
    calibration = False #キャリブレーションを行うときはTrueにする

    if calibration:
        """サーボの位置を固定してキャリブレーションする"""
        rader = Rader_with_VL53L0X(Servo_Ch=0, Servo_ZeroOffset=0, vl53l0x_offset=0, vl53l0x_scale=1, blaket_length=2.5)
        print("{0:s},{1:s}".format("角度[deg]","距離[cm]"))
    else:
        """キャリブレーション結果を反��させて初期化する"""
        rader = Rader_with_VL53L0X(Servo_Ch=0, Servo_ZeroOffset=0, vl53l0x_offset=-1.0, vl53l0x_scale=0.94, blaket_length=2.5)
    try:
        while True:
            if calibration:
                """サーボの位置を固定してキャリブレーションする"""
                dist = rader.GetDistance(90,sleep=0.5)        #サーボを90degに固定して0.5秒sleepする
                print("{0:5.0f},{1:5.1f}".format(90,dist))  #サーボ角と距離をprintする
            else:
                """角度と距離の表示"""
                print("{0:s},{1:s}".format("角度[deg]","距離[cm]"))
                rader.GetDistance(25,sleep=0.5)  #最初のStep時に動きが追いつかないので角度をセットして待つ
                for servo_angle in range(25,156,1):
                    #サーボの位置を移動し距離を取得する
                    dist = rader.GetDistance(servo_angle,sleep=0.001)       #サーボ角をセットし距離を取得する
                    #サーボの角度と距離を表示する
                    print("{0:5.2f},{1:5.2f}".format(servo_angle,dist))     #サーボ角と距離をprintする
                rader.GetDistance(25,sleep=1)  #最初のStep時に動きが追いつかないので角度をセットして待つ

                """x[cm]とy[cm]の表示"""
                print("{0:s},{1:s}".format("x[cm]","y[cm]"))
                rader.GetDistance(25,sleep=0.5)  #最初のStep時に動きが追いつかないので角度をセットして待つ
                for servo_angle in range(25,156,1):
                    #サーボの位置を移動し距離を取得する
                    x,y = rader.GetXY(servo_angle,sleep=0.001)              #対象物のx,y座標を表示
                    #サーボの角度と距離を表示する
                    print("{0:5.2f},{1:5.2f}".format(x,y))                  #対象物のx,y座標を表示

                break

    except KeyboardInterrupt  :         #Ctl+Cが押されたらループを終了
        print("\nCtl+C")
    except Exception as e:
        print(str(e))
    finally:
        rader.Cleanup()
        print("\nexit program")
サンプルプログラム(31_rader_vl53l0x_sg90_92r_with_pca9685.py)実行結果と精度検証結果
『$python 31_rader_vl53l0x_sg90_92r_with_pca9685.py』でサンプルプログラムを実行すると以下の動画のようにサーボを1degずつ動かしながら、障害物までの距離をスキャンしコンソールへ出力します。また、出力した結果をエクセルでグラフに作成してみるとかなり精度が高いことが分かりました。これだけ精度が高ければ、障害物回避だけではなく、マッピングに使えるのではないかと思います。








ċ
31_rader_vl53l0x_sg90_92r_with_pca9685.py
(6k)
1000gou,
2017/07/24 7:06
ċ
VL53L0X_SG90_ブラケット.stl
(115k)
1000gou,
2017/07/24 4:25
Comments