お台場の日本科学未来館で開催されたG空間EXPO2014のデモ走行（11月15日（土））に参加しました。

 

今回のデモ走行は車体をラジコンカーにグレードアップして臨みました（10月18日（土）のGPS・QZSSロボットカーコンテストでは駆動部にダブルギアボックスを使用）。

草刈されたばかりの会場だったこともあり良い走りを期待したのですが、この機体にとっては草が深く、大きく減速してしまうことが何度もありました。

 

パラメーターをぎりぎりまで調整たものの、本番は同じところをぐるぐる回ってばかりいました。方位の取得に使用した$GPRMCではラジコンカー程度の速度だと正確な値が出ないようです。

一度はいい走りをしたかったので残念です。来年は打倒Amano Labです！

Arduinoの開発環境（C言語）でプログラミング

＜参考にしたWebサイト＞

• 2個のモーターを制御できるArduino互換ボードのWebサイト
• GPSロボットカーの作成記録
  
• USBホストシールドの利用方法

// ★は当日変更するパラメーター

 

#include <cdcacm.h>

#include <usbhub.h>

#include "pgmstrings.h"

// Satisfy IDE, which only needs to see the include statment in the ino.

#ifdef dobogusinclude

#include <spi4teensy3.h>

#endif

// QZPODからのデータを格納するバッファーのサイズ

#define READBUFFERSIZE (256)

// 区切り記号

#define DELIMITER (",")

// π

#define pai 3.141592

// NMEAデータ保存用バッファ

char g_szNmea[READBUFFERSIZE] = "";

//Standard PWM DC control

int E1 = 5;     //M1 Speed Control

int E2 = 6;     //M2 Speed Control

int M1 = 4;    //M1 Direction Control

int M2 = 7;    //M1 Direction Control

// マイコンボードのボタンの数

int NUM_KEYS = 5;

// 押されたボタンのNo.

// int adc_key_in=-1;

// キーNo.の保存用

int key=-1;

// RMCを指定された回数受けたら走行制御する

int iCounter = 0;

// 出走番号フラグ

int g_iStatus = 0;

// ★基準点の設定（日本科学未来館） 

float X11 = 13946.5445;

float Y11 = 3537.1497;

float X71 = 13946.53437;

float Y71 = 3537.1445;

float X17 = 13946.53803;

float Y17 = 3537.1580;

// ユニットの緯度経度成分

float Dax = (X71 - X11) / 6;

float Dbx = (X17 - X11) / 6;

float Day = (Y71 - Y11) / 6;

float Dby = (Y17 - Y11) / 6;

// 目的地

float g_wpLong = 0;

float g_wpLat = 0;

// ★車No.

char CarNo[] = "02";

// ターゲットユニット

char g_szTarget[16];

// ★待機フラグ

int iWait =5;

class ACMAsyncOper : public CDCAsyncOper

{

public:

    virtual uint8_t OnInit(ACM *pacm);

};

uint8_t ACMAsyncOper::OnInit(ACM *pacm)

{

    uint8_t rcode;

    // Set DTR = 1 RTS=1

    rcode = pacm->SetControlLineState(3);

    if (rcode)

    {

        ErrorMessage<uint8_t>(PSTR("SetControlLineState"), rcode);

        return rcode;

    }

    LINE_CODING lc;

    lc.dwDTERate = 115200;

    lc.bCharFormat = 0;

    lc.bParityType = 0;

    lc.bDataBits = 8;

    rcode = pacm->SetLineCoding(&lc);

    if (rcode)

        ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);

    return rcode;

}

USB     Usb;

//USBHub     Hub(&Usb);

ACMAsyncOper  AsyncOper;

ACM           Acm(&Usb, &AsyncOper);

void setup()

{

  int i;

  for(i=4;i<=7;i++)

    pinMode(i, OUTPUT);

  

  // LED

  pinMode(13, OUTPUT);

  

  //USBのイニシャライズ

  Usb.Init();

  delay( 200 );

}

void loop()

{

  // ボタンが押されたかを確認する

  int adc_key_in = analogRead(0);

  // S1,S2が押された場合はkeyにコピーする  

  if(adc_key_in <150){//== 143 || adc_key_in == 144 || adc_key_in == 0){

    key = adc_key_in;

    g_iStatus = 0;

  }

  // 目的地に着いた場合はボタンが押されるまで何もしない  

  if(g_iStatus >= 4)

    return;

  

  // S2キーが押されたらロボットカープログラムを実行する

  if(key >10){//== 143 || key == 144){

    ReadQZPOD();

  }

  

  // S1キーが押されたらプログラムを止める

  else if(key == 0){

    stop();

    digitalWrite(13, LOW);

  }

}

// QZPODからデータを読み取る

void ReadQZPOD()

{

  Usb.Task();

  delay(30);

  if(Acm.isReady()) {

    uint8_t rcode;

    uint8_t buf[64];

    uint16_t rcvd = 64;

    //USBから64バイトデータを読み込む

    rcode = Acm.RcvData(&rcvd, buf);

    if( rcvd ) { //more than zero bytes received

      char szReadBuffer[READBUFFERSIZE];

      char szLineString[READBUFFERSIZE];

        

      // 読み込んだデータをszReadBufferにコピーする

      memcpy( szReadBuffer, buf, rcvd );

          

      while(1){

        // 改行データごとにデータを区切る

        if( !ReadLineString( szReadBuffer, rcvd, szLineString, READBUFFERSIZE ) ){

          // 改行データがなかったら前のデータにつなげて更にUSBからデータを読み取るためreturnする

          strcat( g_szNmea, szLineString );

          return;

        }

        // szLineStringの文字列長が0だったら何もしないでreturnする

        if( strlen(szLineString) == 0)

          return;

              

        // 区切りがバッファの中にあった

        // 前半のデータをg_szNmeaに付け足す

        strcat( g_szNmea, szLineString );

//        Serial.println(g_szNmea);

        float fLat = 0.0;

        float fLong = 0.0;

        float fDeg = 0.0;

          

        // NMEAデータを解析する

        int i = AnalyzeLineString( g_szNmea, fLat, fLong, fDeg );

//        Serial.println(i);

        // 災危通報を受信済みの場合のみ以下の処理を実行

        if(g_iStatus >= 1){    

          // g_szNmeaは$QZQSMだったので目標地点が読み込めていない場合は目標ポイントをコピーする

          if(i == 2 && g_wpLong == 0){

            g_wpLat = fLat;

            g_wpLong = fLong;

          }

          // g_szNmeaは$GNRMCだったので走行の処理を行う

          else if( i == 1){

            // ★$GNRMCを指定回もしくはiWaitで指定された回数受信するたびに判断する

            if( iCounter >= 15){//iWait){//10){

              // 測位結果に基づいて車体をコントロールする

              ControlRobotCar(fLat, fLong, g_wpLat, g_wpLong, fDeg);

              iCounter = 1;

              // ★待機フラグ

              iWait = 5;

            }

          }

          else

            iCounter++;

        }

        // 1行の読み取りが終わったのでg_szNmeaのデータをクリアする

        g_szNmea[0] = '\0';

      }

    }

    delay(10);

  }

}

// QZPODから出力されたNMEAデータを1行ずつに切り分ける

// 0 : 読み取り途中。1 : 読み取り完了。

int ReadLineString(char szReadBuffer[], const int ciReadBufferSize, char szLineString[], const int ciLineStringSize )

{

  int iIndexChar = 0;

 

 // 1バイトずつ読み込んでいく 

  while( 1 )

  {

    char c = szReadBuffer[iIndexChar];

    

    // 区切り文字の場合

    if( '\r' == c){

      char buf[ciLineStringSize];

      szReadBuffer[iIndexChar] = '\0';

      

      // szLineStringに区切りの前半のデータををコピーする

      strcpy( szLineString, szReadBuffer);

      if( '\n' == szReadBuffer[iIndexChar+1])

        iIndexChar++;

      

      // bufに区切りの後半のデータをコピーする

      memcpy(buf, &szReadBuffer[iIndexChar+1], ciReadBufferSize - iIndexChar - 1);

      buf[ciReadBufferSize - iIndexChar - 1] = '\0';

      

      // szReadBufferにbufをコピーする

      strcpy(szReadBuffer, buf);

      

      return 1;

    }

    // 区切り文字ではない場合

    else{

      // 最後のデータまで区切り文字がなかった場合

      if( (ciReadBufferSize - 1) <= iIndexChar ){

        // szLineStringにszReadBufferをそのままコピーする

        memcpy( szLineString, szReadBuffer, ciReadBufferSize);

        szLineString[ciReadBufferSize] = '\0';

//        Serial.print("szLineString:");

//        Serial.println(szLineString);

        break;

      }

      iIndexChar++;

    }

  }

  return 0;

}

// センテンスの解析。

// $GPRMCの場合、引数変数に、緯度、経度を入れ、戻り値 1 を返す。

// $QZQSMの場合、引数変数に、ターゲット位置の緯度、経度をいれ、戻り値2を返す。

// それ以外の場合、戻り値は 0 を返す。

// $QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08

// $QZQSM,55,C6C407DEまでは共通、その後の02以降がロボットの番号やメッシュデータと行き先を表す

// 解析例は、次の通り

// 02:指令をうけとるロボットの番号

// 07:緯度方向のユニット分割数、

// 03:ユニット間隔[m]

// 07:経度方向のユニットの分割数、

// 03:ユニット間隔[m]

// 03:目標ターゲット数

// 0101:スタートユニット位置 (1,1)

// 0507: 第一ターゲットの場所（スタート位置から北に5マス目と東に７マス目）

// 0602: 第２ターゲットの場所（北に6マス目と東に2マス目）

// 0205: 第３ターゲットの場所（北に２マス目と東に５マス目）

// 

// 提供されるデータはすべてASCIIのテキストデータ

int AnalyzeLineString( char szLineString[], float& rfLat, float& rfLong, float& rfDeg )

{

  char c0[3], c1[2], c2[2];

  int i0 = 0;

  int Na = 0;

  int Nb = 0;

  // $QZQSMの場合

  // $QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08

  if( 0 == strncmp( "$QZQSM", szLineString, 6 ) ){

    Serial.println( szLineString );

    //  車No.を読み取る

    c0[0] = szLineString[18];

    c0[1] = szLineString[19];

    c0[2] = '\0';

   

    // 指定された車No.の場合

    if(strcmp(c0, CarNo) == 0){

      // 初めての受信のときはフラグをたて、ランプを点灯し、とりあえず方位取得のために走り出す

      if(g_iStatus == 0){

        // ターゲットユニットをメモリにコピー

        memcpy(g_szTarget, &szLineString[34], 12);

        g_szTarget[12] = '\0';

        

        // 1番目のターゲットユニットに向かう

        g_iStatus = 1;

        

        // LEDを点灯する

        digitalWrite(13, HIGH);

        delay(3000);

        digitalWrite(13, LOW);

        

        // とりあえず走り出す

        advance(255,255);

        // ★待機フラグ

        iWait = 10;

      }

    }

    // 指定された車No.以外の場合は動かない、もしくは、止まる

    else{

      stop();

      g_iStatus = 0;

      g_wpLat = 0;

      g_wpLong = 0;

      return 0;

    }

    

    // まだターゲット位置を取得していない場合は取得する

    if( g_wpLat == 0 ){

      c1[0] = g_szTarget[1+(g_iStatus-1)*4];

      c1[1] = '\0';

      c2[0] = g_szTarget[3+(g_iStatus-1)*4];

      c2[1] = '\0';

      Na = atoi(c1);

      Nb = atoi(c2);

      

      // ターゲットユニットの位置情報を算出する

      rfLat = Y11 + Day * (Na-1) + Dby * (Nb-1);

      rfLong = X11 + Dax * (Na-1) + Dbx * (Nb-1);

      return 2;

    }

    else

      return 0;

  }

  // $GPRMCの場合

  // $GNRMC,085120.307,A,3541.1493,N,13945.3994,E,000.0,240.3,181211,,,A*6A

  if( 0 == strncmp( "$GNRMC", szLineString, 6 ) || 0 == strncmp( "$GPRMC", szLineString, 6 ) ){

    Serial.println( szLineString );

    // データを分解する

    strtok( szLineString, DELIMITER ); // $GPRMC

    strtok( NULL, DELIMITER ); // UTC時刻

    strtok( NULL, DELIMITER ); // ステータス

    char* pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm)

    strtok( NULL, DELIMITER ); // 北緯/南緯

    char* pszLong = strtok( NULL, DELIMITER ); // 経度(dddmm.mmmm)

    strtok( NULL, DELIMITER ); // 西経/東経

    strtok( NULL, DELIMITER ); // 対地速度（ノット）

    char* pszDeg = strtok( NULL, DELIMITER ); // 進行方向（真北に対しての度）

    // 位置情報を取れなかった場合

    if( NULL == pszLong )

      return 0;

    // 文字列を数値に変換

    rfLat = atof(pszLat);

    rfLong = atof(pszLong);

    rfDeg = atof(pszDeg);

    

/*    // ★以下災危通報が送信されていない場合のテスト走行用

    c0[0] = '0';

    c0[1] = '5';

    c0[2] = '\0';

   

    // 指定された車No.の場合

    if(strcmp(c0, CarNo) == 0){

      // 初めての受信のときはフラグをたて、ランプを点灯し、とりあえず方位取得のために走り出す

      if(g_iStatus == 0){

        g_iStatus = 1;

        digitalWrite(13, HIGH);

        delay(3000);

        digitalWrite(13, LOW);

        advance(255,255);

        // ★待機フラグ

        iWait = 10;

      }

    }

    // 指定された車No.以外の場合は動かない、もしくは、止まる

    else{

      stop();

      g_iStatus = 0;

      g_wpLat = 0;

      g_wpLong = 0;

      return 0;

    }

    

    // まだターゲット位置を取得していない場合は取得する

    if( g_wpLat == 0 ){

      c1[0] = '6';

      c1[1] = '\0';

      c2[0] = '6';

      c2[1] = '\0';

      Na = atoi(c1);

      Nb = atoi(c2);

      

      // ターゲットユニットの位置情報を算出する

      rfLat = Y11 + Day * (Na-1) + Dby * (Nb-1);

      rfLong = X11 + Dax * (Na-1) + Dbx * (Nb-1);

      return 2;

    }

    // ★テスト用ここまで*/

    

    return 1;    

  }

  

  // その他のNMEAデータの場合

  return 0;

}

// 方位偏差を計算してロボットカーを制御する

// float rbfLat;    ロボットの緯度

// float rbLong;    ロボットの経度

// float wpfLat;    wpの緯度

// float wpLong;    wpの経度

// float d_dir;     ロボットの進行方向（単位：度）

// 戻り値：1 目的地についていない／2 目的地についた

int ControlRobotCar(float& rbLat, float& rbLong, float& wpLat, float& wpLong, float& d_dir)

{

  float jLat = (wpLat - rbLat);  // way point到達判定x値

  float jLong = (wpLong - rbLong);  // way point到達判定y値

  float r_wp;  // way pointの北からの方位

  float r_dir;  // dirのラジアン値

  float d_dev;  // 方位偏差

  // ターゲットユニットに着いた（ユニットサイズの2倍以内にいる）

  if(jLat*jLat + jLong*jLong <= 4*Dax*Dax + 4*Dbx*Dbx){

    // 止まる

    stop();

    

    // 完了のステータス

    g_iStatus++;

    

    // データの初期化

    g_wpLong = 0;

    g_wpLat = 0;

    

    int i = 0;    

    // LEDを光らせる

    while(i < 5){

      digitalWrite(13, HIGH);

      delay(500);

      digitalWrite(13, LOW);

      delay(500);

      i++;

    }

    // まだ3番目のターゲットに到着していない場合はまた走り始める

    if(g_iStatus < 4){

      advance(255,255);

    }

    return 2;

  }

  // 方位の計算

  r_wp = atan((wpLong - rbLong) / (wpLat - rbLat));

  if((wpLong - rbLong) > 0 && (wpLat - rbLat) < 0)

    r_wp = pai + r_wp;

  else if((wpLong - rbLong) < 0 && (wpLat -rbLat) < 0)

    r_wp = pai + r_wp;

  else if((wpLong - rbLong) < 0 && (wpLat -rbLat) > 0)

    r_wp = 2 * pai + r_wp;

  else;

 // 進行方向のラジアン変換

  r_dir = (pai / 180) * d_dir;

  // 方位偏差の計算

  d_dev = r_wp - r_dir;

  if(d_dev <= -pai)

    d_dev = 2 * pai + d_dev;

  // ウェイポイントが進行方向より右側の場合

  if(d_dev > pai / 4 && d_dev <= pai / 2){

    turn_R(255,255);

    delay(750);

    advance(255,255);

    // ★待機フラグ

    iWait = 10;

  }

  else if(d_dev > pai / 2 ){

    turn_R(255,255);

    delay(1500);

    advance(255,255);

    // ★待機フラグ

    iWait = 15;

  }

  // ウェイポイントが進行方向より左側の場合

  else if(d_dev < (-1) * (pai / 4) && d_dev >= (-1) * (pai / 2)){

    turn_L(255,255);

    delay(750);

    advance(255,255);

    // ★待機フラグ

    iWait = 10;

  }

  else if(d_dev < (-1) * (pai / 2)){

    turn_L(255,255);

    delay(1500);

    advance(255,255);

    // ★待機フラグ

    iWait = 15;

  }

  else{

    advance(255,255);

    // ★待機フラグ

    iWait = 5;

  }

  return 1;

}

// 止まる

void stop(void)

{

  digitalWrite(E1,LOW);   

  digitalWrite(E2,LOW);      

}

// 前進する

void advance(char a,char b)

{

  digitalWrite(E1,LOW);

  analogWrite (E2,b);

  digitalWrite(M2,HIGH);

}

// 後進する

void back_off (char a,char b)

{

  digitalWrite(E1,LOW);

  analogWrite (E2,b);

  digitalWrite(M2,LOW);

}

// 左に曲がる

void turn_L (char a,char b)

{

  analogWrite (E1,a);

  digitalWrite(M1,HIGH);

  analogWrite (E2,b);

  digitalWrite(M2,HIGH);

}

// 右に曲がる

void turn_R (char a,char b)

{

  analogWrite (E1,a);

  digitalWrite(M1,LOW);

  analogWrite (E2,b);

  digitalWrite(M2,HIGH);

}