動画はGPSによる目標ポイントを自動走行する動画です。RemoteXY画面から走行中の目標ポイント・距離・方向が表示され、手動走行操作も出来ます。
●GPS処理プログラムについて入江 博樹（熊本高等専門学校）2019年に公開プログラムを参考にしましたが、現在見つけることが出来ませんでした。
動画
車体
●HC-SR04超音波距離センサー
（未使用）
●Ni-MH SC 7.2V 1100mAHバッテリー
●スロットルモーター
●ステアリングサーボ
ブレッドボード
●ESP8266 ESP-12F WiFi 開発ボード （技適製品）
●PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC
●NEO-6M GPSモジュール
●OLED SSD1306 0.96インチ 128 x 64 ディスプレイモジュール
●IoT機器対応 モバイルバッテリー（ESP8266電源供給）
OLED表示
Lat:緯度
wy:目標緯度
Lon:経度
wx:目標経度
direct:方向
dr:距離の差分
dd:方向の差分
wp:ウエイポイント
回路図
RemoteXYアプリ
●同ブログ関連記事：【RemoteXY使い方】
【Properties】【Configuration】設定変更
［Connection］WiFi access point
［Board］ESP8266 based board
［Module］WiFi on chip
【Apply】実行
【Slider】をスマホ画面に貼付け、大きさ・色等を変更
（コントロール配置は５個以内）
【Get source code】実行
コードをArduino IDE エディタ画面に貼付てスケッチを行う。
.png)
joystick_01_x 左折 右折
joystick_01_y 前進 後進
text_01 目標地点
text_02 目標地点までのと距離text_03 北に対する角度
switch_01 手動走行
Arduino IDE
.png)
プログラム
//////////////////////////////////////////////
// RemoteXY include library //
//////////////////////////////////////////////
// you can enable debug logging to Serial at 115200
//#define REMOTEXY__DEBUGLOG
// RemoteXY select connection mode and include library
#define REMOTEXY_MODE__WIFI_POINT
#include <ESP8266WiFi.h>
// RemoteXY connection settings
#define REMOTEXY_WIFI_SSID "RemoteXY"
#define REMOTEXY_WIFI_PASSWORD "12345678"
#define REMOTEXY_SERVER_PORT 6377
#include <RemoteXY.h>
// RemoteXY GUI configuration
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] = // 72 bytes
{ 255,3,0,33,0,65,0,17,0,0,0,82,1,106,200,1,1,5,0,5,
2,2,103,103,0,2,26,31,2,40,168,63,28,0,2,26,31,31,79,78,
0,79,70,70,0,67,6,109,54,14,4,2,26,11,67,6,127,54,14,4,
2,26,11,67,6,145,54,15,4,2,26,11 };
// this structure defines all the variables and events of your control interface
struct {
// input variables
int8_t joystick_01_x; // from -100 to 100
int8_t joystick_01_y; // from -100 to 100
uint8_t switch_01; // =1 if switch ON and =0 if OFF
// output variables
char text_01[11]; // string UTF8 end zero
char text_02[11]; // string UTF8 end zero
char text_03[11]; // string UTF8 end zero
// other variable
uint8_t connect_flag; // =1 if wire connected, else =0
} RemoteXY;
#pragma pack(pop)
/////////////////////////////////////////////
// END RemoteXY include //
/////////////////////////////////////////////
#include <TinyGPS++.h> // Tiny GPS Plus Library
#include <SoftwareSerial.h>
#include <math.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <ESP_Adafruit_SSD1306.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
static const int RXPin = 12, TXPin = 13; // 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
int Jx, Jy, Remote;
#define LEDPin 2
int dist ;
// ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
double wy[16];
double wx[16];
int wp=0; // wp:0-15まで,ウェイポイントの数 -1
int wps=15; // wp ストップ ポイント
int grun=0; // GPS run
double wr=0.15; // ウェイポイントの半径
double ks=0.4; //モーター切れ角比例係数
int led=0;
int gmap=0;
int steer_c=375; // ニュートラルの位置
int steer=1; // ステアリングのch
int posSteer; // ステアリングの位置
float steer_r=300; // ステアリングの最小値
float steer_l=450; // ステアリングの最大値
int throttle=0; //スロットルのch
int throt_f=425; //スロットル前進
int throt_s=375; //スロットル停止
int throt_b=325; //スロットル後進
// 走行処理関係変数
double gy, gx; // 北緯，東経を原点からの距離（メートル）に数値化
double ry, rx, rv, rd; // 北緯（正規化），東経（正規化），速度（ノット），方向（単位：度）の数値化
double dy, dx, dr, dd; // ウェイポイントに対する北緯，東経の差分(m)，距離の差分，方位の差分
int pmode=0; // プログラムモード(メインメニューで分岐)
int dsp=0; // oled 1-GPS 2-WP
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 motorStart(){
pwm.setPWM(throttle, 0, throt_f);
}
//--------------------------------------------------------------------------
// モータストップ
//--------------------------------------------------------------------------
void motorStop(){
pwm.setPWM(throttle, 0, throt_s);
}
//--------------------------------------------------------------------------
// ステアリングニュートラル
//--------------------------------------------------------------------------
void steerNeutral(){
pwm.setPWM(steer, 0, steer_c);
}
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;
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);
}
//--------------------------------------------------------------------------
// 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();
motorStart();
if(kn < 1){
steerNeutral();
}
if (dr < wr){
if(wp>4){
if(wp < wps){
wp=wp+1;
}
}
motorStop();
}
else {
motorStart();
// 角度差に応じて方向にステアリングを切る．
posSteer=steer_c - dd*ks;
posSteer=constrain(posSteer, steer_r,steer_l); // 切れ幅を制限する
pwm.setPWM(steer, 0, posSteer);
// ウェイポイントとの距離を求め，ポイント更新または走行終了判断
}
}
else if(grun==0){
}
}
void led_flash(){
if(led==1){
led=0;
digitalWrite(LEDPin,LOW);
}
else{
led=1;
digitalWrite(LEDPin,HIGH);
}
}
void gsp_oled(){
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();
}
void setup() {
RemoteXY_Init ();
pinMode(LEDPin, OUTPUT);
Serial.begin(9600);
Wire.begin(4,5); // (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);
pwm.begin();
pwm.setPWMFreq(60);
ss.begin(GPSBaud); // Set Software Serial Comm Speed to 9600
map2();
grun=1;
digitalWrite(LEDPin,HIGH);
}
void loop() {
RemoteXY_Handler ();
Jx = RemoteXY.joystick_01_x; //左折 右折
Jy = RemoteXY.joystick_01_y; //前進 後進
Remote = RemoteXY.switch_01; //手動・自動走行切替
//数値→文字列に変換 dtostrf（数値、文字列数、小数点桁数、文字列）
dtostrf(wp, 2, 0, RemoteXY.text_01); //目標地点
dtostrf(dr*64, 10, 1, RemoteXY.text_02); //目標地点までのと距離
dtostrf(dd, 10, 1, RemoteXY.text_03); //北に対する角度
while (ss.available() > 0){
gps.encode(ss.read());
if (gps.location.isUpdated()){
recvGPS();
gsp_oled();
runGPS();
}
}
//手動走行切替
if(Remote == 1 ) {
grun = 0;
pwm.setPWM(throttle, 0, 375+Jy/2); //前進・後進
pwm.setPWM(steer, 0, 375-Jx/2); //左折・右折
}else{
grun = 1;
}
}
0 件のコメント:
コメントを投稿