ソフトウェア

作成中

メモ.
ソフトウェアシリアルでGPSを受信する
ハードウェアシリアルでBluetoothを使ってコマンド操作する
ソフトウェアシリアルでGPSの文字列を待つ.
GPSは1秒おきに数行のデータが出力されるので,1秒以上拘束されない.
1行ごとに拘束から抜けて処理できる.
数行中の必要な行”GPRMC"が受信されたときに,必要なデータを抜き出して航行制御.
それ以外の行では処理を重たくしないようにしないと誤動作.
RCにより強制操縦できるように,RCをモニタ.
RC信号のモニタをpulseInを使って行うと,処理が拘束されるので,RC信号モニタはPICにやらせる.
PICがRC操縦の有無を1ビットのHI,LOWで出力するので,これをArduinoで受けて,航行制御,RC受信を切替える.
ハードウェアシリアルによるBT通信はソフトウェアシリアルに1秒の数分の1待たされる(1秒に数行なので)
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方向への座標)
// 最高8ポイント(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の読み取り,信号がなかったら最大20mSのタイムアウト待ち
  pwMot=pulseIn(MOT_RCV,HIGH,20000);    // ch1の読み取り,信号がなかったら最大20mSのタイムアウト待ち
  
  // -------------- ラダーを動かす -------------- 
  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)	
//  を2進数表示させるために直接制御
//-------------------------------------------------------------------------- 
void dispLED(byte n)
{
 PORTB &= B11000111;                     // LEDを一旦消す
 PORTB |= (n << 3);                        // LED表示
}

// LEDを指定した表示で5回点滅させる
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;       //    
  }
}