GPSロボットカーのソフトウェア
ソフトウェア作成中
メモ．
ソフトウェアシリアルでGPSを受信する
ハードウェアシリアルでBluetoothを使ってコマンド操作する
ソフトウェアシリアルでGPSの文字列を待つ．
GPSは１秒おきに数行のデータが出力されるので，1秒以上拘束されない．
1行ごとに拘束から抜けて処理できる．
数行中の必要な行”GPRMC"が受信されたときに，必要なデータを抜き出して航行制御．
それ以外の行では処理を重たくしないようにしないと誤動作．
RCにより強制操縦できるように，RCをモニタ．
RC信号のモニタをpulseInを使って行うと，処理が拘束されるので，RC信号モニタはPICにやらせる．
PICがRC操縦の有無を１ビットのHI,LOWで出力するので，これをArduinoで受けて，航行制御，RC受信を切替える．
ハードウェアシリアルによるBT通信はソフトウェアシリアルに１秒の数分の１待たされる（１秒に数行なので）
BTによるコマンド受信はバッファ内に収まる文字数で送っておくと，拘束を抜けてから受信処理される．
航行中は，GPSによる航行制御またはRC操縦，BT通信，を繰り返す．
// *****************************************************************************
#include < Servo.h>
#include < EEPROM.h>
#include < SoftwareSerial.h> // ライブラリの導入
#include < math.h> // atn，pow関数用
// デジタルピンの定義
#define rxPin 2 // ソフトウェアシリアルポートのRX
#define txPin 3 // ソフトウェアシリアルポートのTX
#define SET 4 // セットスイッチ，デジタル入力
#define START 5 // スタートスイッチ，デジタル入力
#define RUD_RCV 6 // ラジコン受信 ch1 ，どちらかをPICのGP1へ
#define MOT_RCV 7 // ラジコン受信 ch2
#define RC_MON 8 // ラジコン送信機のON-OFFモニタ用，PICのGP2より
#define RUD_SRV 9 // サーボ出力 ラダー
#define MOT_OUT 10 // サーボ出力 DCDC出力のON-OFF
#define LED0 11 // 表示用LED（PB3）
#define LED1 12 // 表示用LED（PB4）
#define LED2 13 // 表示用LED（PB5）
#define DEF_WR 2 // ウェイポイントの半径
#define DEF_KS 0.2 // ラダー切れ角比例係数
#define RUDMIN 10 // ラダー右最大切れ角
#define RUDMAX 170 // ラダー左最大切れ角
#define POSC 90 // 停止時のサーボの制御値
#define PI/2 1.5708 // 3.14159/2
#define PI/180 0.01745 // 3.14159/180
// char strA[20]; // Android等Bluetooth通信用文字列バッファ
char str[100]; // GPSほか読み取りのための文字列バッファ
char *cmd1, *cmd2;
char *latitude,*longitude, *knot, *direct;
char latitS[12],longitS[12];
double latit, longit, kn; // 緯度，経度，ノット保存用の変数
// ウェイポイント設定(基準点からのN,E方向への座標)
// 最高８ポイント（LEDの表示可能範囲より）,座標はあらかじめ定義
double wy[] = { 3257.0666, 3252.6122, 3252.6122, 3252.6122, 3252.6122, 3252.6122, 3252.6122, 3252.6122};
double wx[] = {13040.5495,13044.9423,13044.9423,13044.9423,13044.9423,13044.9423,13044.9423,13044.9423};
int wp=7; // wp:0-7までウェイポイントの数 -1
int np=0; // 目標ポイントナンバー
float wr=DEF_WR; // ウェイポイントの半径
float ks=DEF_KS; // ステアリング切れ角比例係数
// 走行処理関係変数
double gy, gx; // 北緯，東経を原点からの距離（メートル）に数値化
float ry, rx, rv, rd; // 北緯（正規化），東経（正規化），速度（ノット），方向（単位：度）の数値化
float dy, dx, dr, dd; // ウェイポイントに対する北緯，東経の差分(m)，距離の差分，方位の差分
int pmode=0; // プログラムモード(メインメニューで分岐)
int pwRud, pwMot; // パルス幅記録用変数
int posRud=90; // サーボ,アンプコントロール用
int motor=0; // 0:モータストップ，1:自動制御（0,1ともRCは可）
// ラダーサーボ制御用オブジェクト生成
Servo rudder;
// ソフトウェアシリアルポートを設定
SoftwareSerial mySerial = SoftwareSerial(rxPin, txPin);
//-------------------------各種関数定義----------------------------------------
// ソフトウェアシリアルで文字列受信．受信がなければ待ち続ける
void recvStrS(char *buf)
{
int i = 0;
char c;
while (1) {
c = mySerial.read();
buf[i] = c;
if (c == '\n') break;
i++;
}
buf[i] = '\0'; // \0: end of string
}
// ハードウェアシリアルで文字列受信．
void recvStr(char *buf)
{
int i = 0;
char c;
while (1) {
if (Serial.available()) {
c = Serial.read();
buf[i] = c;
if (c == '\n') break;
i++;
}
}
buf[i] = '\0'; // \0: end of string
}
//--------------------------------------------------------------------------
// GPSによる航行
//--------------------------------------------------------------------------
void run_gps(){
recvStrS(str); // ソフトウェアシリアルで文字列受信．受信がなければ待ち続ける
if(strcmp(strtok(str,","),"$GPRMC")==0){ //if RMC line
strtok(NULL,",");
strtok(NULL,",");
latitude=strtok(NULL,","); //get latitude
strtok(NULL,",");
longitude=strtok(NULL,","); //get longtude
strtok(NULL,","); // E読み飛ばし
knot=strtok(NULL,","); // 速度読み出し
direct=strtok(NULL,","); // 進行方向読み出し
strncpy(latitS,latitude,strlen(latitude)); // 実数化すると桁落ちするので表示用に文字列を保管
strncpy(longitS,longitude,strlen(longitude));
latit=atof(latitude); // 文字列を実数に変換．有効数字が足りないので1m弱の誤差がでる．
longit=atof(longitude); // 文字列を実数に変換．有効数字が足りないので1m弱の誤差がでる．
kn=atof(knot);
rv=kn/2; // ノット単位をメートル単位に変換(正確には*0.51)
rd=atof(direct); // 方位を数値変換
// 緯度経度の数値変換，絶対位置で表す場合
dy=(wy[np] - latit ) *1860;
dx=(wx[np] - 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;
// 方位偏差の計算し，現在の進行方向から±180度の範囲に直す
dd=dd-rd;
if (dd > 180) dd=dd-360;
else if (dd < -180) dd=dd+360;
// 角度差に応じて方向に舵を切る．速度により値を可変？
posRud=POSC-dd*ks;
posRud=constrain(posRud, RUDMIN,RUDMAX); // 切れ幅を制限する
// ウェイポイントとの距離を求め，ポイント更新または走行終了判断
if (dr < wr){
posRud=POSC;
if (np < wp) { np++; Serial.print("np: "); Serial.println(np); }
}
dispLED(np);
// ラダー，スロットルを動かす
rudder.write(posRud); // ラダーは航行中動かしっぱなし
if (np < wp){
pinMode(MOT_OUT, OUTPUT); digitalWrite(MOT_OUT,LOW); // R/C端子を短絡状態でモータ回す
} else {
pinMode(MOT_OUT, INPUT); // 走行終了してたらモータ止める
}
}
}
//--------------------------------------------------------------------------
// RCによる航行
//--------------------------------------------------------------------------
void run_rc(){
pwRud=pwMot=0; // パルス幅の読み取り値の初期化
pwRud=pulseIn(RUD_RCV,HIGH,20000); // ch1の読み取り，信号がなかったら最大20ｍSのタイムアウト待ち
pwMot=pulseIn(MOT_RCV,HIGH,20000); // ch1の読み取り，信号がなかったら最大20ｍSのタイムアウト待ち
// -------------- ラダーを動かす --------------
posRud=map(pwRud, 500,1500, 0, 180); // パルス幅をサーボ制御値に変換
rudder.write(posRud);
// ------- モータ動かす．パルス幅が規定値以上ならモータON，それ以外ならOFF -------
if (pwMot < 1000){
pinMode(MOT_OUT, OUTPUT); digitalWrite(MOT_OUT,LOW); // R/C端子を短絡状態
} else pinMode(MOT_OUT, INPUT); // R/C端子を絶縁状態
}
//--------------------------------------------------------------------------
// Bluetoothによる外部機器との通信．例えばAndroidによるデータの送信など．
//--------------------------------------------------------------------------
void BT_com(){
// ハードウェアシリアルで文字列受信，受信がなければ抜ける
if (Serial.available()) {
recvStr(str);
cmd1=strtok(str,", ");
cmd2=strtok(NULL,", ");
Serial.println(cmd1);
Serial.println(cmd2);
// ”ALL”コマンド:GPS航行にかかる数値を全て出力
if(strcmp(cmd1,"ALL")==0){
Serial.print("latitude "); Serial.println(latitS); // 実数化したものは桁落ちしてるので，
Serial.print("longitude "); Serial.println(longitS); // 文字列で表示
Serial.print("knot "); Serial.println(kn);
Serial.print("direct "); Serial.println(rd);
Serial.print("dx "); Serial.println(dx);
Serial.print("dy "); Serial.println(dy);
Serial.print("dr "); Serial.println(dr);
Serial.print("dd "); Serial.println(dr);
Serial.print("rv "); Serial.println(rv);
}
// ”SENS”コマンド：指定センサ（引数：0-5）または全センサ（引数6以上）の測定値を出力
if(strcmp(cmd1,"SENS")==0){
switch(atoi(cmd2)){ // 指定されたセンサの値を送る
case 0: Serial.print("AN0:"); Serial.println(analogRead(0)); break;
case 1: Serial.print("AN1:"); Serial.println(analogRead(1)); break;
case 2: Serial.print("AN2:"); Serial.println(analogRead(2)); break;
case 3: Serial.print("AN3:"); Serial.println(analogRead(3)); break;
case 4: Serial.print("AN4:"); Serial.println(analogRead(4)); break;
case 5: Serial.print("AN5:"); Serial.println(analogRead(5)); break;
default:
Serial.print("AN0:"); Serial.println(analogRead(0)); // 全センサ測定
Serial.print("AN1:"); Serial.println(analogRead(1));
Serial.print("AN2:"); Serial.println(analogRead(2));
Serial.print("AN3:"); Serial.println(analogRead(3));
Serial.print("AN4:"); Serial.println(analogRead(4));
Serial.print("AN5:"); Serial.println(analogRead(5));
break;
}
}
}
}
//--------------------------------------------------------------------------
// 航行ルーチン
//--------------------------------------------------------------------------
void run()
{
while (digitalRead(START)==HIGH){ // STARTスイッチが押されたら抜ける
if (digitalRead(RC_MON)==LOW){ // RC入力の有無で制御方法を分岐
run_gps(); // GPSによる航行
} else {
run_rc(); // R/C操縦
}
BT_com(); // Bluetoothによる外部機器との通信．例えばAndroidによるデータの送信など．
}
while (digitalRead(START)==LOW); // STARTを離すまで待つ
}
//--------------------------------------------------------------------------
// LED表示
// LED2(digital13:PB5),LED1(digital12:PB4),LED0(digital11:PB3)
// を２進数表示させるために直接制御
//--------------------------------------------------------------------------
void dispLED(byte n)
{
PORTB &= B11000111; // LEDを一旦消す
PORTB |= (n << 3); // LED表示
}
// LEDを指定した表示で５回点滅させる
void blink(int n){
int i;
for (i=0;i < 5;i++){ dispLED(n); delay(100); dispLED(0); delay(100); }
}
//--------------------------------------------------------------------------
// セットアップ以外の初期値設定
//--------------------------------------------------------------------------
void initPara(){
pinMode(MOT_OUT, INPUT); // R/C端子を絶縁状態
np=0; // ウェイポイントの座標をリセット
}
//--------------------------------------------------------------------------
// セットアップ
//--------------------------------------------------------------------------
void setup()
{
pinMode(SET, INPUT); // セットスイッチ
pinMode(START, INPUT); // スタートスイッチ
pinMode(LED1, OUTPUT); // 状態モニタ用LED
pinMode(LED2, OUTPUT); // 状態モニタ用LED
pinMode(rxPin, INPUT); // ソフトウェアシリアルRX端子
pinMode(txPin, OUTPUT); // ソフトウェアシリアルTX端子
pinMode(RUD_RCV, INPUT); // 受信機ラダー信号監視用
pinMode(MOT_RCV, INPUT); // 受信機モータ信号監視用
pinMode(RC_MON , INPUT); // 送信機ON-OFFのモニタ用
pinMode(MOT_OUT, INPUT); // モーター出力は絶縁か短絡．INPUT設定で初期は絶縁状態に
rudder.attach(RUD_SRV); // ラダーサーボ制御用ピン設定
// シリアル通信の初期化
mySerial.begin(9600); // ソフトウェアシリアルの速度を設定（遅く）
Serial.begin(115200); // ハードウェアシリアルの速度を設定（できるだけ早く）
}
//--------------------------------------------------------------------------
// メインループ
//--------------------------------------------------------------------------
void loop()
{
initPara();
blink(7);
while (digitalRead(START)==HIGH){ // スタートスイッチが押されたら指定プログラムを実行
if (digitalRead(SET)==LOW) { // セットスイッチが押されたらプログラムモードを進める
delay(10); // チャタリング防止
while(digitalRead(SET)==LOW);
delay(10); // チャタリング防止
pmode++; if (pmode > 7) pmode=0;
}
dispLED(pmode); // プログラムモードをLEDに2進表示
// BT_com(); // ここで頻繁に出力をかけると暴走するので注意 // Bluetoothによる外部機器との通信．例えばAndroidによるデータの送信など．
}
dispLED(0);
delay(500); // 0.5秒待つ
switch(pmode){ // プログラム実行
case 0: run(); break; // 航行ルーチン呼び出す
case 1: break; //
case 2: break; //
case 3: break; //
case 4: break; //
case 5: break; //
case 6: break; //
case 7: break; //
}
}
 ESP32 Wifi Bluetooth開発ボード |
 Arduino Nano Every |
 Raspberry Pi pico |
 FPGA XILINX Artix-7 |