複数のウェイポイント（走行位置）をGPSデータの緯度・経度から方位、距離を計算して順番に走行します。
●GPS処理プログラムは当時、入江 博樹（熊本高等専門学校）公開プログラムを参考にしました。
●ESP-32 NodeMCU開発ボード（技適製品）
●OLED SSD1306 0.96インチ 128 x 64 ディスプレイモジュール
●4WD L298N DC モータードライバ
●3.7V充電電池ｘ２
ESP32・モーター給電
GPS・センサー
●GPSモジュール（秋月電子）
●QMC5883L 3軸磁気センサー
コンパス センサー
●VL53L0Xレーザー距離センサー
障害物を検知すると一旦バック して再走行します。
OLED表示
Lat:緯度
wy:目標緯度
Lon:経度
wx:目標経度
direct:方向
dr:距離の差分
dd:方向の差分
wp:ウエイポイント
図面
Arduino IDE
.png)
プログラム
#include <math.h> // atn，pow関数用
#include <TinyGPS++.h>
#include <DFRobot_QMC5883.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <ESP_Adafruit_SSD1306.h>
#include <VL53L0X.h>
VL53L0X gVL530X;
int dist;
#define OLED_RESET 4
#define RXD2 16
#define TXD2 17
TinyGPSPlus gps;
Adafruit_SSD1306 display(OLED_RESET);
DFRobot_QMC5883 compass;
float headingDegrees;
int maxX = 2550;
int maxY = 7480;
int minX = -10150;
int minY = -5060;
//int offX = -6466;
//int offY = 4349;
int offX = 0;
int offY = 0;
//モータードライバDRV8833の接続
#define ENA 14
#define ENB 27
#define IN1 25
#define IN2 26
#define IN3 32
#define IN4 33
#define LEDPin 19
// ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
double wy[16];
double wx[16];
int wp=0; // wp:0-15まで,ウェイポイントの数 -1
int wps=15; // wp ストップ ポイント
double wr=0.05; // ウェイポイントの半径
double ks=5; // もーたー切れ角比例係数
int spd=255; // car スピード
int spc; // スピード制御
int spcmin=0; // 最小値
int spcmax=spd; // 最大値
int led=0;
int gmap=0; //google map wp
// 走行処理関係変数
double gy, gx; // 北緯，東経を原点からの距離（メートル）に数値化
double ry, rx, rv, rd; // 北緯（正規化），東経（正規化），速度（ノット），方向（単位：度）の数値化
double dy, dx, dr, dd; // ウェイポイントに対する北緯，東経の差分(m)，距離の差分，方位の差分
int pmode=0; // プログラムモード(メインメニューで分岐)
int dsp=1; // oled 1-GPS 2-WP
int deg_lat,deg_lon;
// GPS受信用変数，ポインタ
float latitude, longitude, mps, direct;
double latit, longit, kn; // 緯度，経度，ノット保存用の変数
double latitB, longitB, rdB; // 差分で方位計算する時に前の座標保持用, 方位保管用
double dy2, dx2, rd2; // 一つ前の値との北緯，東経の差分(m)，差分で求めた方位
void map2() {
digitalWrite(LEDPin,HIGH);
gmap=1; // google map wp mode
wp=5; // wp start point
wps=13; // wp stop point
wy[5] = 38.**4281; //運動場駐車場2
wx[5] = 141.**0814;
wy[6] = 38.**4187;
wx[6] = 141.**0833;
wy[7] = 38.**4185;
wx[7] = 141.**0661;
wy[8] = 38.**4118;
wx[8] = 141.**0676;
wy[9] = 38.**4142;
wx[9] = 141.**0845;
wy[10] = 38.**4024;
wx[10] = 141.**0865;
wy[11] = 38.**4018;
wx[11] = 141.**0686;
wy[12] = 38.**4276;
wx[12] = 141.**0635;
wy[13] = 38.**4306;
wx[13] = 141.**0821;
wy[14] = 0;
wx[14] = 0;
wy[15] = 0;
wx[15] = 0;
delay(2000);
digitalWrite(LEDPin,LOW);
}
void Stop(){
ledcWrite(1, 0); // stop
ledcWrite(2, 0);
}
void R_forward(){
ledcWrite(1, spd - spc);
ledcWrite(2, spd );
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
void L_forward(){
ledcWrite(1, spd );
ledcWrite(2, spd - spc);
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
void led_flash(){
if(led==1){
led=0;
digitalWrite(LEDPin,LOW);
}
else{
led=1;
digitalWrite(LEDPin,HIGH);
}
}
void gsp_oled(){
if(dsp==0){
display.clearDisplay();
display.setTextSize(1);
display.setCursor(0,0);
display.print("Latitude :");
display.println(gps.location.lat(), 6);
display.print("Longitude :");
display.println(gps.location.lng(), 5);
display.print("Satellites:");
display.println(gps.satellites.value());
display.print("Elevation :");
display.print(gps.altitude.feet());
display.println("ft");
display.print("Time UTC :");
display.print(gps.time.hour()); // GPS time UTC
display.print(":");
display.print(gps.time.minute()); // Minutes
display.print(":");
display.println(gps.time.second()); // Seconds
//display.print("course :");
//display.println(gps.course.deg());
display.print("direct :");
display.println(direct);
display.print("Speed km/h:");
display.println(gps.speed.kmph());
display.print("distance :");
display.println(dist);
display.display();
}
if(dsp==1){
display.clearDisplay();
display.setTextSize(1);
display.setCursor(0,0);
display.print("Lat : ");
display.println(latitude, 6);
display.print("wy[wp]: ");
display.println(wy[wp], 6);
display.print("Lon : ");
display.println(longitude, 6);
display.print("wx[wp]: ");
display.println(wx[wp], 6);
display.print("direct: "); // 方向
display.println(direct);
display.print("dr : "); // 目標地点までのと距離
display.println(dr*64);
display.print("dd : "); // 北に対する角度
display.println(dd);
display.print("wp : "); // 目標地点
display.print(wp);
display.display();
}
}
//--------------------------------------------------------------------------
// GPS受信
//-------------------------------------------------------------------
void recvGPS(){
latitude=gps.location.lat(); //get latitude
longitude=gps.location.lng(); //get longtude
kn=gps.speed.knots(); // 速度 get
//direct=gps.course.deg(); // 進行方向 get
gps_val(); // GPS信号の数値返還
gps_cal(); // 現在地とウェイポイントとの関係を計算
delay(1);
}
//--------------------------------------------------------------------------
// GPS信号の数値返還
//--------------------------------------------------------------------------
void gps_val(){
latitB=latit; longitB=longit; // 一つ前の値を保管
latit=latitude;
longit=longitude;
deg_lat=(int)( latitude/100); //
deg_lon=(int)( longitude/100); //
rv=mps;
rdB=rd; // 一つ前の値を保管
rd=direct; // 方位を数値変換
}
void gps_cal(){
// 緯度経度の数値変換，目標地点との差分を計算
dy=(wy[wp] - latit )*1860;
dx=(wx[wp] - longit)*1560;
// 目標地点までのと距離を計算
dr=sqrt(pow(dy,2)+pow(dx,2));
// 目標地点への方向を計算
dd = atan(dx / dy); // 北に対する角度を求める(±π/2範囲)
dd=dd*57; // ラジアン->度に変換 dd*(180/pai)
// 0-360度に変換
if (dx > 0 && dy < 0) dd = 180 + dd;
else if(dx < 0 && dy < 0) dd = 180 + dd;
else if(dx < 0 && dy > 0) dd = 360 + dd;
else;
// GPSが正しい方位を出力しない場合は、前の座標との差分で計算
dy2=(latit -latitB) *1860;
dx2=(longit-longitB)*1560;
if (dy2==0 || dx2==0){ // 緯度または経度のどちらかの変化が０の時
if (dy2==0){ if (dx2<0) rd2= 270; else rd2=90; } // rd2= 0, 90, 180，270 のいずれかを直接与える．
if (dx2==0){ if (dy2<0) rd2=-180; else rd2=0; } // dy2,dx2のどちらも0の時はrd2=0
} else {
// 目標地点への方向を計算
rd2 = atan(dx2 / dy2); // 北に対する角度を求める(±π/2範囲)
rd2=rd2*57; // ラジアン->度に変換 dd*(180/pai)
// 0-360度に変換
if (dx2 > 0 && dy2 < 0) rd2 = 180 + rd2;
else if(dx2 < 0 && dy2 < 0) rd2 = 180 + rd2;
else if(dx2 < 0 && dy2 > 0) rd2 = 360 + rd2;
else;
}
// 移動距離が小さくて使用するGPSが正しい速度と方向を出さない時のみ
// 座標差から計算した値と置き換える（速度=0,方位前の値のまま，GPSの仕様により異なる）
if (kn==0 && dx2!=0 && dy2!=0){
rd=rd2;
}
// 方位偏差の計算し，現在の進行方向から±180度の範囲に直す
dd=dd-rd;
if (dd>180) dd=dd-360;
else if (dd<-180) dd=dd+360;
}
//--------------------------------------------------------------------------
// GPSによる走行
// runMode=0:GPSにより方位が取れなくても偏差で計算
// runMode=1:方位がとれない場合はニュートラルに戻す
//--------------------------------------------------------------------------
void runGPS(){
led_flash();
spc=abs(dd)*ks;
spc=constrain(spc,spcmin,spcmax);
if (dr < wr){
if(wp>4){
if(wp < wps){
wp=wp+1;
}
}
Stop();
}
else{
if(dd >= 0){
R_forward();
}
else if(dd < 0){
L_forward();
}
}
}
void recvCompass(){
Vector mag = compass.readRaw();
// Serial.print(mag.XAxis);
// Serial.print(",");
// Serial.print(mag.YAxis);
// Serial.print(",");
// Serial.print(mag.ZAxis);
// Serial.println(",");
if (mag.XAxis > maxX) maxX = mag.XAxis;
if (mag.YAxis > maxY) maxY = mag.YAxis;
if (mag.XAxis < minX) minX = mag.XAxis;
if (mag.YAxis < minY) minY = mag.YAxis;
// Serial.print(maxX);
// Serial.print(",");
// Serial.print(maxY);
// Serial.print(",");
// Serial.print(minX);
// Serial.print(",");
// Serial.println(minY);
Vector norm = compass.readNormalize();
// Calculate heading
float heading = atan2(norm.YAxis, norm.XAxis);
float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
heading += declinationAngle;
// Correct for heading < 0deg and heading > 360deg
if (heading < 0) {
heading += 2 * PI;
}
if (heading > 2 * PI) {
heading -= 2 * PI;
}
// Convert to degrees
float headingDegrees = heading * 180 / PI;
// Output
//// Serial.print(" Heading = ");
//Serial.print(heading);
//Serial.print(",");
//// Serial.print(" Degress = ");
//Serial.print(headingDegrees);
//Serial.println();
direct=headingDegrees; // 進行方向 get
}
void vl53(){
dist = gVL530X.readRangeContinuousMillimeters();
if (gVL530X.timeoutOccurred()){
}
if(dist < 400 ){
ledcWrite(1, spd); // back
ledcWrite(2, spd);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
delay(1000);
ledcWrite(1, spd); // left
ledcWrite(2, spd);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(500);
ledcWrite(1, spd); // go
ledcWrite(2, spd);
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(500);
Stop();
}
}
void setup() {
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(LEDPin, OUTPUT);
map2();
digitalWrite(LEDPin,HIGH);
ledcSetup(1,490,8); // モータ 0CH 490Hz 8bit=0-255(PWM)
ledcSetup(2,490,8);
ledcAttachPin(ENA, 1); // モータ 0CH
ledcAttachPin(ENB, 2);
delay(1000);
Wire.begin(21,22); // (SDA,SCL):ESP8266(IO_0)-OLED(SDA),(IO_1)-OLED(SCL)
display.begin(SSD1306_SWITCHCAPVCC, 0x78>>1); // OLED ADDRESS
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(WHITE); // Set OLED color to White
display.setCursor(0,0); // Set cursor to 0,0
display.println("GPS example");
display.println(TinyGPSPlus::libraryVersion());
display.display();
delay(1500); // Pause 1.5 seconds
gVL530X.init();
gVL530X.setTimeout(1000);
gVL530X.startContinuous(100);
//Serial.begin(9600);
Serial2 . begin ( 9600 , SERIAL_8N1 , RXD2 , TXD2 ) ;
while (!compass.begin()) {
//Serial.println("Could not find a valid QMC5883 sensor, check wiring!");
delay(500);
//yield();
}
if(compass.isHMC()){
compass.setRange(HMC5883L_RANGE_1_3GA);
compass.setMeasurementMode(HMC5883L_CONTINOUS);
compass.setDataRate(HMC5883L_DATARATE_15HZ);
compass.setSamples(HMC5883L_SAMPLES_8);
}
if (compass.isQMC()) {
compass.setRange(QMC5883_RANGE_2GA);
compass.setMeasurementMode(QMC5883_CONTINOUS);
compass.setDataRate(QMC5883_DATARATE_50HZ);
compass.setSamples(QMC5883_SAMPLES_8);
}
}
void loop() {
while (Serial2.available() > 0) {
char c = Serial2.read();
gps.encode(c);
if (gps.location.isUpdated()) {
recvCompass(); //direct コンパス方向検出
recvGPS();
gsp_oled();
runGPS();
vl53();
}
}
}
0 件のコメント:
コメントを投稿