RaspberryPi 3 Model B+と赤外線距離センサー（GP2Y0A02YK）で距離を測定する

赤外線距離センサー

RaspberryPi 3 Model B+にシャープの赤外線距離センサー（GP2Y0A02YK）を接続して距離を測定するプログラムを作成した時の備忘録。

尚、この赤外線距離センサーはアナログ信号を返すのでラズパイで直接処理をする事ができないので、アナログ信号をデジタル信号に変換するADコンバーター（MPC3208）を使っている。

記事後半でADコンバーターからの電圧を取得するまでのロジックについて説明している。

また同様の内容の動画を記事の最後にリンクを張っている。

ADコンバーター等の説明は動画の方が分かりやすいかも知れないので確認してみて欲しい。

今回の記事で使用した赤外線距離センサーのスペックは以下の通り

上記の他に１０ｃｍ～８０ｃｍの測定が可能なGP2Y0A21も一緒に購入したのでそのうちに試してみようと思っている。

また今回は物体との距離を測定するのみたがAWS IoT Coreに接続してリアルタイムの測定結果をクラウドにパブリッシュ（アップロード）することもできる。

IoT Coreとの接続はこちらの記事を参考にしてほしい。

接続方法

GP2Y0A02YKの出力はアナログなのでADコンバーター（MCP3208）経由でRaspberryPiに接続する。

またGP2Y0A02YKに標準で付属しているリード線は何故かVcc(５V）が黒、GNDが茶色、Vo（output）は白なので注意が必要。

SPIの有効化

事前にSPIを有効にする。

RaspberryPiのメニューから設定ー＞Raspberry Piの設定。

インターフェースタブでSPIを有効にして「OK」で保存後にRaspberryPiを再起動する。

spidevのインストール

以下のコマンドでspidevをインストールした。

sudo apt-get install python-spidev

プログラム

プログラムは以下の通り（解説は後述）

# -*- coding: utf-8 -*-
import time                                             # time(sleepを使うためのモジュール)のインポート
import spidev                                           # spidev(spi通信を行うためのモジュール)のインポート
import numpy as np                                      # 配列
CHN = 0                                                 # MCP3208のCH（0～7）
VOLTS = 5                                               # 電圧（3.3v or 5v）

class  ADConverterClass:                                # AD Converter（MCP3208）から値を取得するクラス
    def __init__(self, ref_volts, ch):                  # コンストラクタ
        self.ref_volts = ref_volts                      # 電圧セット
        self.spi = spidev.SpiDev()                      # SpiDevのインスタンスを作成
        self.spi.open(0,0)                              # 0：SPI0、0：CE0
        self.spi.max_speed_hz = 1000000                 # 1MHz SPIのバージョンアップによりこの指定をしないと動かない!
        self.ch = ch                                    # GP2Y0A02のMCP3208への入力チャンネル

    def get_voltage(self, ch):                          # A/Dコンバータ（MCP3208）で電圧を取得する
        raw = self.spi.xfer2([((0b1000+ch)>>2)+0b100,((0b1000+ch)&0b0011)<<6,0])  # Din(RasPi→MCP3208）
        raw2 = ((raw[1]&0b1111) << 8) + raw[2]          # Dout（MCP3208→RasPi）
        volts = (raw2 * self.ref_volts ) / float(4095)  # 取得した値を電圧に変換する（12bitなので4095で割る）
        volts = round(volts,4)                          # 電圧を4桁に四捨五入する
        return volts                                    # 電圧を返す

    def get_dist(self):                                 # GP2Y0A02で距離を測定する
        v_rate = np.array([[2.55,2.75,0.18,20],         # ボルトと距離との関係の表
                          [2,2.55,0.18,30],             # From Volts、To Volts、変換率、距離
                          [1.55,2,0.22,40],
                          [1.25,1.55,0.33,50],
                          [1.05,1.25,0.5,60],
                          [0.9,1.05,0.67,70],
                          [0.81,0.9,1.11,80],
                          [0.72,0.81,1.11,90],
                          [0.64,0.72,1.25,100],
                          [0.59,0.64,2,110],
                          [0.54,0.59,2,120],
                          [0.5,0.54,2.5,130],
                          [0.46,0.5,2.5,140],
                          [0.43,0.46,3.33,150],
                          [0,0.43,100,4450]])
        volts = self.get_voltage(self.ch)               # MCP3208のGP2Y0A02を入力したチャンネルの電圧を取得する
        if volts > v_rate[0,1]:                         # 指定したVoltの範囲外の時
            dist = 0
        elif volts < v_rate[len(v_rate)-1,1]:
            dist = 999
        else:
            for i in range(len(v_rate)):
                if v_rate[i,0] <= volts and volts < v_rate[i,1]: # FromとToの範囲内のVoltを検索する
                    bs_volt = v_rate[i,0]
                    cm_rate = v_rate[i,2]
                    cm_base = v_rate[i,3]
                    break
            dist = cm_base -((volts - bs_volt)*cm_rate*100)     # 距離を計算する
        return dist                                             # 距離を返す

    def Cleanup(self):                                          # 終了処理
        self.spi.close()                                        # SpiDevの終了処理

if __name__ == '__main__': # main
    ad_conv = ADConverterClass(ref_volts=VOLTS, ch=CHN) # Classのインスタンス作成 
    try:
        while True:
            __volts = ad_conv.get_voltage(ch=CHN)       # ボルトを取得して表示
            print("volts: {:8.2f}".format(__volts))

            __dist = ad_conv.get_dist()                 # GP2Y0A02から推定した距離[cm]を取得する
            print("GP2Y0A02の距離[cm]: {:8.2f}".format(__dist))  #コンソールに表示

            time.sleep(1)                               # スリープ
    except KeyboardInterrupt:                           # Ctl+C]
        print("\nCtl+C")
    except Exception as e:
        print(str(e))                                   # 例外処理の内容をコンソールに表示
    finally:
        ad_conv.Cleanup()                               # ADConverterClassの終了処理
        print("\nexit program")                         # プログラム終了

距離の計算方法について

GP2Y0A02YKは赤外線で測定した距離を電圧で出力するので電圧から距離を算出する必要がある。

GP2Y0A02YKのデータシートの３ページ目に電圧と距離の関係を表すグラフが載っていた。

縦軸が電圧で横軸が距離（ｃｍ）で距離が遠くなるにつれて電圧が低くなっているのだが線形（直線）ではないので単純に電圧の何らかの係数を掛けて算出できるわけではない。※1次関数で表すことができない。

近似曲線を算出しようとしたのだがExcelだと簡単に近似曲線が算出できるようなのだが残念ながらExcelは持っていない。

そのうちに誰かに借りて試してみようとは思っているがまずは別の方法で出来ないかを検討してみる。

よく見ると１０ｃｍ単位では（ほぼ）直線なので電圧によって傾きを変えれば近似値を１次関数で計算できそうなので、その方法でいくことにした。

上記の例だと２．５５ｖ～２ｖの時は２０ｃｍ～３０ｃｍの間で直線（１次関数）で近似できる。

以下、２ｖ～１．５５ｖの時は３０ｃｍ～４０ｃｍの間で・・・と続く。

原始的だけど、グラフをA4用紙に印刷してさしがねで値を測定した。

そして作ったのが２４～３８行目の配列。

v_rate = np.array([[2.55,2.75,0.18,20],         # ボルトと距離との関係の表
                  [2,2.55,0.18,30],             # From Volts、To Volts、変換率、距離
                  [1.55,2,0.22,40],
                  [1.25,1.55,0.33,50],
                  [1.05,1.25,0.5,60],
                  [0.9,1.05,0.67,70],
                  [0.81,0.9,1.11,80],
                  [0.72,0.81,1.11,90],
                  [0.64,0.72,1.25,100],
                  [0.59,0.64,2,110],
                  [0.54,0.59,2,120],
                  [0.5,0.54,2.5,130],
                  [0.46,0.5,2.5,140],
                  [0.43,0.46,3.33,150],
                  [0,0.43,100,4450]])

配列の値は左から

1. From電圧
2. To電圧
3. 変換率（傾き）
4. 元となる距離

の４つの要素からなっている。

ADコンバータから取得した電圧からFrom、Toの範囲内の行を検索して、マッチした行の３．変換率（傾き）と４．元となる距離を取得する。

変換率（傾き）は２０ｃｍ～３０ｃｍ（差は１０ｃｍ）の距離で電圧が２．５５V～２V（差は０．５５V）なので１０÷０．５５÷１００＝０．１８（小数点以下３桁目を四捨五入）で設定している。

そして５０行目の計算式で距離を算出する。

dist = cm_base -((volts - bs_volt)*cm_rate*100)     # 距離を計算する

例えばADコンバータからの電圧が１．３Vだった時は上から１．２５V～１．５５Vの範囲内なので４行目の

[1.25,1.55,0.33,50]

がヒットして、

1. From電圧：１．２５ ｖ
2. To電圧：１．５５ ｖ
3. 変換率：０．３３ ｃｍ／ｖ
4. 元となる距離：５０ ｃｍ

となる。

１．３ｖー１．２５ｖ＝０．０５ｖ

０．０５ｖ×０．３３ｃｍ／ｖ（変換率）×１００＝１．６５ｃｍ

５０ｃｍ（元となる距離）ー１．６５ｃｍ＝４８．３５ｃｍ（測定したい距離）

と計算した。

MCP3208

ADコンバータ（MCP3208）から電圧を取得するロジックについての備忘録。

raw = self.spi.xfer2([((0b1000+ch)>>2)+0b100,((0b1000+ch)&0b0011)<<6,0])  # Din(RasPi→MCP3208）
raw2 = ((raw[1]&0b1111) << 8) + raw[2]          # Dout（MCP3208→RasPi）
volts = (raw2 * self.ref_volts ) / float(4095)  # 取得した値を電圧に変換する（12bitなので4095で割る）

１行目はRaspberryPiからMCP3208にチャンネルを指定する信号を送っている。

２行目はMCP3208からRaspberryPiで電圧を１２ｂｉｔで受け取っている。

３行目は２行目で受けた値を電圧に変換している

１行目の解説

まずはMCP3208のデータシートを参照。

１４ページの表５－２がMCP3208の構成ビット。

データシートより

上記の表５－２よりCH0の時は0b1000、CH1は0b1001・・・と指定する事が分かる。

次に１６ページの図６－１（spiモード0,0）のDinを見ると８ビットづつ３セットでチャンネルを指定することが分かる。

データシートより

仮にCH1にGP2Y0A02YKを繋いだ場合、

0b00000110, 0b01000000, 0b00000000

とビットを指定したい（図６－１のDinを参照）

(0b1000+ch)

で表５－２のSINGLE/DIFF D2 D1 D0のビットを生成して>>2で後半の２ビット（D1、D0）を削除、更に+0b100で１セット目の８ビット（0b00000110）を作成している。

次に(0b1000+ch)までは先程と同様。

(0b1000+ch)&0b0011

で後半２ビットの論理積（AND演算）で前半の２ビット（SINGLE/DIFF、D2）を0にして、<<6で左に６ビットシフトして２セット目の８ビット（0b01000000）を作成している。

３セット目の８ビットは無視されるビットなのでとりあえず０をセットした。

２行目の解説

Doutはraw[0]、raw[1]、raw[2]に８ビットづつセットされるのでraw[0]は不要、raw[1]の後半４ビットとraw[2]の８ビットだけを使用する。

raw[1]の後半４ビットとraw[2]の８ビットをつなげて合計１２ビットを作りたい。

raw[1]は

raw[1]&0b1111

で後半４ビットの論理積（前半４ビットを無視）後、<< 8で左に８ビットシフトしてraw[2]を加えて合計１２ビットの値を得る。

３行目の解説

１２ビット＝２の１２乗＝４，０９６＝０～４，０９５の値を取る。

５V（ref_volts）が４，０９５になるので４，０９５で割ることにより電圧を求める。

その他

その他にハマったのは１３行目の

self.spi.max_speed_hz = 1000000                 # 1MHz SPIのバージョンアップによりこの指定をしないと動かない!

この行が無いと正しい電圧が取得されなかった。

インターネット上の過去のプログラムではこの行が無いプログラムが多いのだがどうやらspidevがバージョンアップして仕様が変わった模様なので要注意。

実際の計測値との比較

RaspberryPiの箱を置いて実際に計測をしてみた。

計測を３回行い平均と比較したときの結果は以下の通り。

およそ３％以内の誤差の範囲に収まっているが、３回の計測の中でも多少のばらつきがある事が分かる。

また対象物との距離が長くなるとばらつきが増すようにも感じた。

対象物をRaspberryPiの箱から別のモノに変えた所、微妙に測定結果が変わったので対象物の材質や色によっても違いが出てくるのだと思う。

またはGP2Y0A02YKの個体差も多少はあるのかも知れない。

常に同じ材質のモノとの距離を測定するのであれば前述の配列表の値を変えてやれば、より誤差の少ない測定が可能になると思われる。

動画について

この記事の内容をYoutubeで解説している。

ADコンバーターの説明は動画の方が理解しやすいと思うので上記の説明で良く分からない人は見て欲しい。