2020年に製作したESP8266によるGPSロボットカー自動走行について記事にしました。
複数のウェイポイント（走行位置）をGPSデータの緯度・経度から方位、距離を計算して順番に走行します。
●GPS処理プログラムは当時、入江 博樹（熊本高等専門学校）公開プログラムを参考にしました。
ESP8266開発ボード
 |
●モータドライバ DRV8833
●OLED SSD1306 0.96インチ 128 x 64 ディスプレイモジュール
●モバイルバッテリー 5000mAh（ESP8266開発ボード電源）
NEO-6M GPSモジュール
●NEO-6M GPSモジュール GY-GPS6MV2 (青）  |
| 走行モーター |
 |
| ステアリングモーター |
モータ電源
OLED表示
Lat:緯度
Lon:経度
direct:方向
mph:スピード
dd:方位の差分
dr:距離の差分
wp:ウエイポイント
回路図
Arduino IDE
※プログラム書込み時、GPSモジュールTX RXジャンパーピンを外すこと！
.png)
プログラム
#include <math.h> // atn，pow関数用
#include <TinyGPS++.h> // Tiny GPS Plus Library
#include <SoftwareSerial.h> // Software Serial Library so we can use other Pins for communication with the GPS module
#include <Adafruit_ssd1306syp.h> // https://github.com/stevenvo/arduino-libraries/tree/master
Adafruit_ssd1306syp display(4,5);
float Turn = 85;
float Run_Speed = 0;
//モータードライバDRV8833の接続
#define AIN1 12
#define AIN2 13
#define AIN3 14
#define AIN4 15
#define LEDPin 2
static const int RXPin = 3, TXPin = 1; // Ublox 6m GPS module to pins 12 and 13
static const uint32_t GPSBaud = 9600; // Ublox GPS default Baud Rate is 9600
const double Home_LAT = 00.000000; // Your Home Latitude
const double Home_LNG = 00.000000; // Your Home Longitude
TinyGPSPlus gps; // Create an Instance of the TinyGPS++ object called gps
SoftwareSerial ss(RXPin, TXPin); // The serial connection to the GPS device
// ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
double wy[16];
double wx[16];
int wp=0; // wp:0-15まで,ウェイポイントの数 -1
int wps=15; // 目標ポイントナンバー
int grun=0; // GPS run
double wr=0.1; // ウェイポイントの半径
double ks=5; // もーたー切れ角比例係数
int spd=1023; // car スピード
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 deg_lat,deg_lon;
// GPS受信用変数，ポインタ
float latitude, longitude, mph, 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 led_flash(){
if(led==1){
led=0;
digitalWrite(LEDPin,LOW);
}
else{
led=1;
digitalWrite(LEDPin,HIGH);
}
}
void gsp_oled(){
display.clear();
display.setCursor(0,0);
display.print("Lat : ");
display.println(latitude, 6);
display.print("Lon : ");
display.println(longitude, 6);
display.print("direct : ");
display.println(direct);
display.print("mph : ");
display.println(mph);
display.print("dd : ");
display.println(dd);
display.print("dr : ");
display.println(dr);
display.print("np : ");
display.println(wp);
display.update(); // Update 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(); // 現在地とウェイポイントとの関係を計算
}
//--------------------------------------------------------------------------
// GPS信号の数値返還
//--------------------------------------------------------------------------
void gps_val(){
latitB=latit; longitB=longit; // 一つ前の値を保管
latit=latitude;
longit=longitude;
deg_lat=(int)( latitude/100); //
deg_lon=(int)( longitude/100); //
rv=mph;
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 < 1 && 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(){
if(grun==1){
led_flash();
analogWrite(AIN1, 0); // go
analogWrite(AIN2, spd);
if(kn < 1){
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
if (dr < wr){
if(wp>4){
if(wp < wps){
wp=wp+1;
}
}
analogWrite(AIN1, 0); // stop
analogWrite(AIN2, 0);
delay(300);
}
else{
if(dd >= 90){
analogWrite(AIN3, spd); // right
analogWrite(AIN4, 0);
delay(500);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd < 90 && dd >= 60 ){
analogWrite(AIN3, spd); // right
analogWrite(AIN4, 0);
delay(400);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd < 60 && dd >= 45 ){
analogWrite(AIN3, spd); // right
analogWrite(AIN4, 0);
delay(300);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd < 45 && dd >= 30 ){
analogWrite(AIN3, spd); // right
analogWrite(AIN4, 0);
delay(200);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd < 30 && dd >= 15){
analogWrite(AIN3, spd); // right
analogWrite(AIN4, 0);
delay(100);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd <= -90){
analogWrite(AIN3, 0); // left
analogWrite(AIN4, spd);
delay(500);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd > -90 && dd <= -60){
analogWrite(AIN3, 0); // left
analogWrite(AIN4, spd);
delay(400);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd > -60 && dd <= -45){
analogWrite(AIN3, 0); // left
analogWrite(AIN4, spd);
delay(300);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd > -45 && dd <= -30){
analogWrite(AIN3, 0); // left
analogWrite(AIN4, spd);
delay(200);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
else if(dd > -30 && dd <= -15){
analogWrite(AIN3, 0); // left
analogWrite(AIN4, spd);
delay(100);
analogWrite(AIN3, 0); // center
analogWrite(AIN4, 0);
}
}
}
else if(grun==0){
}
}
void setup() {
pinMode(AIN1,OUTPUT);
pinMode(AIN2,OUTPUT);
pinMode(AIN3,OUTPUT);
pinMode(AIN4,OUTPUT);
analogWrite(AIN1, 0);
analogWrite(AIN2, 0);
analogWrite(AIN3, 0);
analogWrite(AIN4, 0);
pinMode(LEDPin, OUTPUT);
delay(1000);
display.initialize(); // Initialize OLED display
display.clear(); // Clear OLED display
display.setTextSize(1); // Set OLED text size to small
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.update(); // Update display
delay(1500); // Pause 1.5 seconds
ss.begin(GPSBaud); // Set Software Serial Comm Speed to 9600
map2();
grun=1;
digitalWrite(LEDPin,HIGH);
}
void loop() {
while (ss.available() > 0){
gps.encode(ss.read());
if (gps.location.isUpdated()){
recvGPS();
gsp_oled();
runGPS();
}
}
}
0 件のコメント:
コメントを投稿