JA7MLC ブログ: GPSロボットカー Arduino Uno自動走行

2019年に製作したArduino UnoによるGPSロボットカー自動走行について記事にしました。

複数のウェイポイント（走行位置）をGPSデータの緯度・経度から方位、距離を計算して順番に走行します。

●GPS処理プログラムは当時、入江博樹（熊本高等専門学校）公開プログラムを参考にしました。

ラジコンカー4WD

●ラジコンカー4WD

NEO-6M GPSモジュール

●Arduino Uno

●NEO-6M GPSモジュール

●DRV8833モータードライバ

●I2C接続の小型LCD搭載ボード(5V版)

スイッチサイエンス(Switch Science)

電源

●9V電池リチウムイオン充電池

　Arduino 電源供給

●Ni-Cd 700mAh 4.8V　充電池

　モータ電源供給

モーターステアリング+モーター駆動（4WD)

図面

Arduino IDE

プログラム

#include <TinyGPS++.h> // Tiny GPS Plus Library

#include <SoftwareSerial.h>

#include <Adafruit_TiCoServo.h>

#include <math.h>

#include <Wire.h>

#define I2Cadr 0x3e // 固定

byte contrast = 25; // コントラスト(0～63)

//モータードライバDRV8833の接続

#define AIN1 5

#define AIN2 6

#define AIN3 9

#define AIN4 10

#define LEDPin 13

static const int RXPin = 17, TXPin = 16; // Ublox 6m GPS module to pins 12 and 13

static const uint32_t GPSBaud = 9600; // Ublox GPS default Baud Rate is 9600

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; // wp ストップポイント

int grun=0; // GPS run

double wr=0.05; // ウェイポイントの半径

double ks=0.1; // もーたー切れ角比例係数

int spd=255; // car スピード

int led=0;

int gmap=0;

// 走行処理関係変数

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)，差分で求めた方位

// Forward

void forward(){

analogWrite(AIN1, 0); // go

analogWrite(AIN2, spd);

}

// Back

void back(){

analogWrite(AIN1, spd); // back

analogWrite(AIN2, 0);

}

// Left

void left(){

analogWrite(AIN3, 0); // left

analogWrite(AIN4, spd);

}

// Right

void right(){

analogWrite(AIN3, spd); // right

analogWrite(AIN4, 0);

}

//Stop

void stop(){

analogWrite(AIN1, 0); // stop

analogWrite(AIN2, 0);

analogWrite(AIN3, 0);

analogWrite(AIN4, 0);

}

//--------------------------------------------------------------------------

// モータスタート

//--------------------------------------------------------------------------

void motorStart(){

analogWrite(AIN1, 0); // go

analogWrite(AIN2, spd);

}

//--------------------------------------------------------------------------

// モータストップ

//--------------------------------------------------------------------------

void motorStop(){

analogWrite(AIN1, 0); // stop

analogWrite(AIN2, 0);

analogWrite(AIN3, 0);

analogWrite(AIN4, 0);

}

//--------------------------------------------------------------------------

// ステアリングニュートラル

//--------------------------------------------------------------------------

void steerNeutral(){

}

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);

}

//--------------------------------------------------------------------------

// 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);

}

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(300);

analogWrite(AIN3, 0); // center

analogWrite(AIN4, 0);

}

else if(dd < 60 && dd >= 45 ){

analogWrite(AIN3, spd); // right

analogWrite(AIN4, 0);

delay(200);

analogWrite(AIN3, 0); // center

analogWrite(AIN4, 0);

}

else if(dd < 45 && dd >= 30 ){

analogWrite(AIN3, spd); // right

analogWrite(AIN4, 0);

delay(150);

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(300);

analogWrite(AIN3, 0); // center

analogWrite(AIN4, 0);

}

else if(dd > -60 && dd <= -45){

analogWrite(AIN3, 0); // left

analogWrite(AIN4, spd);

delay(200);

analogWrite(AIN3, 0); // center

analogWrite(AIN4, 0);

}

else if(dd > -45 && dd <= -30){

analogWrite(AIN3, 0); // left

analogWrite(AIN4, spd);

delay(150);

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 led_flash(){

if(led==1){

led=0;

digitalWrite(LEDPin,LOW);

}

else{

led=1;

digitalWrite(LEDPin,HIGH);

}

void lcd_cmd(byte x) {

Wire.beginTransmission(I2Cadr);

Wire.write(0b00000000); // CO = 0,RS = 0

Wire.write(x);

Wire.endTransmission();

}

void lcd_clear()

{

lcd_cmd(0b00000001);

}

void lcd_contdata(byte x) {

Wire.write(0b11000000); // CO = 1, RS = 1

Wire.write(x);

}

void lcd_lastdata(byte x) {

Wire.write(0b01000000); // CO = 0, RS = 1

Wire.write(x);

}

// 文字の表示

void lcd_printStr(const char *s) {

Wire.beginTransmission(I2Cadr);

while (*s) {

if (*(s + 1)) {

lcd_contdata(*s);

} else {

lcd_lastdata(*s);

}

s++;

}

Wire.endTransmission();

}

// 表示位置の指定

void lcd_setCursor(byte x, byte y) {

lcd_cmd(0x80 | (y * 0x40 + x));

}

void lcd_printInt(int num)

{

char int2str[10];

sprintf(int2str,"%d",num);

lcd_printStr(int2str);

}

void lcd_dd(){

lcd_clear();

lcd_setCursor(0, 0);

lcd_printStr("w");

lcd_printInt(wp);

lcd_printStr("c");

lcd_printInt(rd);

lcd_setCursor(0, 1);

lcd_printStr("d");

lcd_printInt(dd);

lcd_printStr("s");

lcd_printInt(kn);

}

void setup() {

Serial.begin(9600);

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);

Wire.begin();

lcd_cmd(0b00111000); // function set

lcd_cmd(0b00111001); // function set

lcd_cmd(0b00000100); // EntryModeSet

lcd_cmd(0b00010100); // interval osc

lcd_cmd(0b01110000 | (contrast & 0xF)); // contrast Low

lcd_cmd(0b01011100 | ((contrast >> 4) & 0x3)); // contast High/icon/power

lcd_cmd(0b01101100); // follower control

delay(200);

lcd_cmd(0b00111000); // function set

lcd_cmd(0b00001100); // Display On

lcd_cmd(0b00000001); // Clear Display

delay(2);

lcd_setCursor(0, 0);

lcd_printStr("GPS_CAR");

lcd_setCursor(0, 1);

lcd_printStr("AUTO_RUN");

delay(100);

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();

lcd_dd();

runGPS();

}

JA7MLC ブログ

2024年10月1日火曜日

GPSロボットカー Arduino Uno自動走行

0 件のコメント:

コメントを投稿

Raspberry Pi Donkey Car スマートカー

不正行為を報告

ラベル