Arduinoで簡単なナビを自作してみよう！

2017.09.29 04:17

～簡易ナビゲーション端末「Geoquest」について～

1．コンセプト

　近年、スマートフォンの普及により、行ったことのない目的地へも素早く移動出来るようになりました。その反面、私たちは経路を自ら選択せずに機械から与えられた道だけを進むようになっています。今回、使用者に目的地への方向だけを示しつつ、使用者が自らの感覚により道を選択するような装置を開発し、街の風景を楽しみながら移動することをコンセプトとしています。

2．外観

3．構成部品

Arduino UNO
GPSモジュール(GY-GPS6MV2)
HMC5883L搭載　地磁気センサ(GY-271)
16桁2行LCD
赤色LED
タクトスイッチ
９V乾電池
その他配線類

4．回路図

0－ピンクーDB7【LCD】
1ー橙色ーDB6【LCD】
2－黄色ーDB5【LCD】
3－緑色ーDB4【LCD】
4－水色ーE【LCD】
5－青色ーRS【LCD】
6－茶色(上)ーTX【GPS】
7－茶色(下)ーRX 【GPS】　
8－LED－GND
15(A1)－灰色(下)ータクトスイッチ(右)　　
16(A2)－灰色(中)ータクトスイッチ(中央)
17(A3)－灰色(上)ータクトスイッチ(左)　　
５V ーVDD【LCD】　　
GNDーVSS【LCD】
GND－抵抗(1KΩ)ーVO【LCD】　　
GNDーR/W【LCD】
Arduinoと地磁気センサはSDA、SCLどうしを接続する。

5．制御プログラム

5.1　ライブラリ

　使用したライブラリとその主な関数は以下の通りです。

「SoftwareSerial.h」GPSモジュール用

SoftwareSerial.available()：ソフトウェアシリアルポートに何バイトのデータが到着しているかを返します。

SoftwareSerial.read()：ソフトウェアシリアルポートから１文字読み取ります。

SoftwareSerial(rxPin,txPin)：rxPinとtxPinを設定します。

SoftwareSerial.begin(speed)：speed(ボーレート)を設定します。

「TinyGPS++.h」GPSモジュール用

gps.encode(chracter)：到着した文字(chracter)をArduinoに送信します。

gps.location.isValid()：位置情報が変化しているかを示します。

gps.location.lat()：現在地の緯度を返します。

gps.location.lng()：現在地の経度を返します。

「Wire.h」地磁気センサ用
「DFRobot＿QMC5883.h」地磁気センサ用(HMC5883Lを搭載したGY-271でも使用可能)
「LiquidCrystal.h」LCD用

lcd.begin(桁,行)：ディスプレイの桁数と行数を指定します。

lcd.setCursor(桁,行)：カーソルの位置を指定します。

lcd.print(data)：dataの文字列を返します。

lcd.clear()：LCDの画面をクリアする。

5.2　プログラムの流れ

　まず、初めにGeoquestにはGPSモジュールから取得した緯度経度をLCDに表示させる「現在地表示モード」と目的地の緯度経度を入力しLED点灯によって目的地の方向を示す「ナビゲーションモード」があります。以下PCでの閲覧を推奨いたします。

5.2.1　現在地表示モード

　「現在地表示モード」はkeicode.com様のプログラムを参考にし、以下のように記述しました。

while(ss.available() > 0){
    gps.encode(ss.read());
    if(gps.location.isUpdated()){
    String s;
   s="lat=";
   s+=String(gps.location.lat(),6);　//緯度(小数点以下6桁)
    lcd.setCursor(0,0);　//LCDのカーソルを0桁0行に設定
    lcd.print(s);　//LCDに表示
    s="lng=";
    s+=String(gps.location.lng(),6);　//経度(小数点以下6桁)
    lcd.setCursor(0,1);　//LCDのカーソルを0行1行に設定
    lcd.print(s);　//LCDに表示
    }
}

5.2.2　ナビゲーションモード～目的地の位置情報を入力する～

　次に「ナビゲーションモード」に関してですがデフォルトの目的地を兵庫県西脇市にある日本へそ公園(北緯35度東経135度)とし、左右のタクトスイッチを押すとその位置情報が増減するように記述しています。

if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
   delay(50);
   def_lat+=0.00001;　//デフォルト目的地の緯度に0.00001を足す
   s=def_lat;
   lcd.setCursor(0,0);
   lcd.print("mokutekiti-ido");
   lcd.setCursor(0,1);
   lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
   delay(50);
   def_lat-=0.00001;　//デフォルト目的地の緯度に0.00001を引く
   s=def_lat;
   lcd.setCursor(0,0);
   lcd.print("mokutekiti-ido");
   lcd.setCursor(0,1);
   lcd.print(s);
}

　※経度に関しても同様に記述しています。

5.2.3　ナビゲーションモード～設定の切り替え～

　中央のタクトスイッチで目的地緯度設定→目的地経度設定→目的地方向表示のように切り替えるプログラムを作成しました。その一部のプログラムが以下の通りです。

val=0;
/*目的地緯度設定のプログラム*/
if(digitalRead(c_sw)==HIGH){　//中央のタクトスイッチが押されたとき
delay(50);　//チャタリング防止
val=1-val; //valを１にする
}
while(val==1){
　/*目的地経度設定のプログラム*/
}

5.2.4　ナビゲーションモード～角度計算～

　最後に目的地の方向の角度計算についてご紹介します。

角度計算プログラムは以下のようになっています。

while(ss.available() > 0){
    gps.encode(ss.read());
    while(gps.location.isUpdated()){
        /*角度計算プログラム*/
        float gps_lat=gps.location.lat();    //現在地の緯度
        float gps_lng=gps.location.lng();    //現在地の経度
        float a_ang=atan2((gps_lng-def_lng),(gps_lat-def_lat))*180/PI;
　　 //北を0度としたときの目的地の角度

　　 a_ang=map(a_ang,-180,180,0,360);    //-180~180を0~360に変換
        //DFRobot様のサンプルプログラムを引用
        Vector norm=compass.readNormalize();
        float heading=atan2(norm.YAxis,norm.XAxis);
        float declinationAngle=((-7.0)+(27.0/60.0))/(180/PI);
　　//地磁気センサの補正(補正値は使用する場所で異なる。)
        heading+=declinationAngle;
        if(heading<0){
          heading+=2*PI;
        }
        if(heading>2*PI){
          heading-=2*PI;
        }
        float headingDegrees=heading*180/PI;    //北を0度としたときの使用者の向く角度
        float ang=0.0;
        ang=headingDegrees-a_ang;//使用者が目的地まで何度向けばよいかを計算
        /*LED点灯プログラム(目的地の方向を向くと点灯)*/
        if(((0<=ang)&&(ang<=10))||((350<=ang)&&(ang<=360))){
           digitalWrite(led,HIGH);    //LED点灯
        }else{
            digitalWrite(led,LOW);    //LED消灯
        }
        /*LCD*表示プログラム/
        String s="ang=";
        s+=String(a_ang);
        lcd.setCursor(0,0);
        lcd.print(s);    //北を0度としたときの目的地の角度を表示
        String s1="my_ang=";
        s1+=String(headingDegrees);
        lcd.setCursor(0,1);
        lcd.print(s1);    //北を0度としたときの使用者の向く角度を表示
        delay(500);
    }
}

5.2.5　動作プログラム

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <DFRobot_QMC5883.h>
#include <Wire.h>
#include <LiquidCrystal.h>
TinyGPSPlus gps;
SoftwareSerial ss(6,7);//(rx,tx)
DFRobot_QMC5883 compass;
LiquidCrystal lcd(5,4,3,2,1,0);//(rs,e,db4,db5,db6,db7)
/*---+LEDピン+---*/
int led=8;
/*---+スイッチピン+---*/
int r_sw=15;
int c_sw=16;
int l_sw=17;
/*---+スイッチ判定用変数+---*/
int g_val=0;
int m0_val=0;
int m01_val=0;
int m02_val=0;
int m1_val=0;
int m11_val=0;
int m12_val=0;
int m2_val=0;
/*---+デフォルト目的地座標(日本へそ公園)+---*/
float def_lat=35.0000;
float def_lng=135.0000;

void setup(){
/*---+初期設定+---*/
/*---+LCD初期設定+---*/
lcd.begin(16,2);//(桁,行)
/*---+GPSシリアル通信+---*/
ss.begin(9600);
/*---+地磁気センサ初期設定+---*/
compass.setRange(QMC5883_RANGE_2GA);
compass.setMeasurementMode(QMC5883_CONTINOUS);
compass.setDataRate(QMC5883_DATARATE_50HZ);
compass.setSamples(QMC5883_SAMPLES_8);
/*---+LED初期設定+---*/
pinMode(led,OUTPUT);
digitalWrite(led,LOW);
/*---+スイッチ初期設定設定+---*/
pinMode(r_sw,INPUT);
pinMode(c_sw,INPUT);
pinMode(l_sw,INPUT);
/*---+タイトル+---*/
lcd.setCursor(0,0);
lcd.print("Geoquest");
delay(3000);
}

void loop(){
/*---+メニュー+---*/
lcd.setCursor(0,0);
lcd.print("Location >>");
lcd.setCursor(0,1);
lcd.print("<< Navi");
/*---+スイッチ判定+---*/
if(digitalRead(r_sw)==HIGH){
delay(50);
lcd.clear();
g_val=1-g_val;
}else if(digitalRead(l_sw)==HIGH){
delay(50);
lcd.clear();
m0_val=1-m0_val;
}
/*---+現在地位置情報取得・出力モード+---*/
while(g_val==1){
while(ss.available() > 0){
gps.encode(ss.read());
if(gps.location.isUpdated()){
String s;
s="lat=";
s+=String(gps.location.lat(),6);//緯度
lcd.setCursor(0,0);
lcd.print(s);
s="lng=";
s+=String(gps.location.lng(),6);//経度
lcd.setCursor(0,1);
lcd.print(s);
}
}
}
/*---+ナビゲーションモード+---*/
/*....目的地緯度設定....*/
while(m0_val==1){
String s;
s=String(def_lat,4);
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
delay(50);
if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
delay(50);
def_lat+=0.0001;
s=def_lat;
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
delay(50);
def_lat-=0.0001;
s=def_lat;
lcd.setCursor(0,0);
lcd.print("Goal-lat");
lcd.setCursor(0,1);
lcd.print(s);
}else if(digitalRead(c_sw)==HIGH){
delay(50);
lcd.clear();
m01_val=1-m01_val;
}
while(m01_val==1){
String s;
s=String(def_lat,4);
lcd.setCursor(0,0);
lcd.print("Goal-lat");
lcd.setCursor(0,1);
lcd.print(s);
delay(50);
if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
delay(50);
def_lat+=0.01;
s=def_lat;
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
delay(50);
def_lat-=0.01;
s=def_lat;
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
}else if(digitalRead(c_sw)==HIGH){
delay(50);
lcd.clear();
m02_val=1-m02_val;
}
while(m02_val==1){
String s;
s=String(def_lat,4);
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
delay(50);
if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
delay(50);
def_lat++;
s=def_lat;
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lat)&&(def_lat<=180.000)){
delay(50);
def_lat--;
s=def_lat;
lcd.setCursor(0,0);
lcd.print("Goal-Lat");
lcd.setCursor(0,1);
lcd.print(s);
}else if(digitalRead(c_sw)==HIGH){
delay(50);
lcd.clear();
m1_val=1-m1_val;
}
/*....目的地経度設定....*/
while(m1_val==1){
String s;
s=String(def_lng,4);
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
delay(50);
if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lng)&&(def_lng<=180.000)){
delay(50);
def_lng+=0.0001;
s=def_lng;
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lng)&&(def_lng<=180.000)){
delay(50);
def_lng-=0.0001;
s=def_lng;
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
}else if(digitalRead(c_sw)==HIGH){
delay(50);
lcd.clear();
m11_val=1-m11_val;
}
while(m11_val==1){
String s;
s=String(def_lng,4);
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
delay(50);
if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lng)&&(def_lng<=180.000)){
delay(50);
def_lng+=0.01;
s=def_lng;
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lng)&&(def_lng<=180.000)){
delay(50);
def_lng-=0.01;
s=def_lng;
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
}else if(digitalRead(c_sw)==HIGH){
delay(50);
lcd.clear();
m12_val=1-m12_val;
}
while(m12_val==1){
String s;
s=String(def_lng,4);
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
delay(50);
if((digitalRead(r_sw)==HIGH)&&(-180.000<=def_lng)&&(def_lng<=180.000)){
delay(50);
def_lng++;
s=def_lng;
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
}else if((digitalRead(l_sw)==HIGH)&&(-180.000<=def_lng)&&(def_lng<=180.000)){
delay(50);
def_lng--;
s=def_lng;
lcd.setCursor(0,0);
lcd.print("Goal-Lng");
lcd.setCursor(0,1);
lcd.print(s);
}else if(digitalRead(c_sw)==HIGH){
delay(50);
lcd.clear();
m2_val=1-m2_val;
}
/*....角度計算....*/
while(m2_val==1){
while(ss.available() > 0){
gps.encode(ss.read());
while(gps.location.isUpdated()){
float gps_lat=gps.location.lat();//緯度
float gps_lng=gps.location.lng();//経度
float a_ang=atan2((gps_lng-def_lng),(gps_lat-def_lat))*(180/PI);
a_ang=map(a_ang,-180,180,0,360);
while (!compass.begin()){
lcd.setCursor(0,0);
lcd.println("check wiring!");
delay(50);
}
Vector norm=compass.readNormalize();
float heading=atan2(norm.YAxis,norm.XAxis);
float declinationAngle=((-7.0)+(27.0/60.0))/(180/PI);
heading+=declinationAngle;
if(heading<0){
heading+=2*PI;
}
if(heading>2*PI){
heading-=2*PI;
}
float headingDegrees=heading*180/PI;
float ang=0.0;
ang=headingDegrees-a_ang;
if(((-10<=ang)&&(ang<=0))||((0<=ang)&&(ang<=10))){//LED点灯
digitalWrite(led,HIGH);
}else{
digitalWrite(led,LOW);
}
String s="ang=";
s+=String(a_ang);
lcd.setCursor(0,0);
lcd.print(s);
String s1="my_ang=";
s1+=String(headingDegrees);
lcd.setCursor(0,1);
lcd.print(s1);
delay(500);
}
}
}
}
}
}
}
}
}
}

6．改善できていない問題点と機能の追加

現在地表示モードからナビゲーションモードの移行、またはその逆を行うことができない点。
最初の位置情報取得に数分かかってしまう点。(仕方ないかもしれない)
目的地までの距離の表示する機能の実装。

7．今後のアップデートと新たな製品の開発

　上記の問題点を改善するために今後も情報収集を行っていき、ユーザーが快適に使用できるようアップデートをおこなっていきたいと考えています。また、そのことと並行して、新たにスマートフォンとBluetoothで通信する全く新しい文字入力デバイスの開発をしていく予定です。ご期待ください。

8．参考にさせて頂いたページ

TinyGPS++ | Arduiniana

GPS モジュール (NEO-6M) からの GPS データを液晶ディスプレイに表示する方法 - 基礎からの IoT 入門

DFRobot/DFRobot_QMC5883

電子コンパスの使い方(GY-273,HMC5883L) - NOBのArduino日記！

【Arduino】SC1602B(16桁LCDディスプレイ)を使ってみる。 | Blazing Azuki's Blog

この場を借りてお礼申し上げます。ありがとうございました。