ロボット・インフィニティ（TypeA）の課題を解決する2つの対策をしました。

 

----------

1. ロボットカーの向きを正確に把握できるように地磁気センサーを搭載

以前はGPSモジュール（QZPOD）から出力される「$GPRMC」に含まれる「進行方向」を「ロボットカーの向き」として利用していた。$GPRMCの「進行方向」はある程度の速度がないと出力されないうえに、精度が低かった。そこで、地磁気センサーを搭載し、ロボットカーが静止していても正確な方位が取得できるようにした。

 

2. ロボットカーの状態を把握できるようにシリアル通信できるBluetoothモジュールを搭載

以前は離れた場所からロボットカーの状態が把握できず、デバッグしづらかった。（前回のブログでLCDシールドを搭載したが、ロボットカーと一緒に移動しないとLCDが確認できない上、ログが残らず不便だった。）そこで、ロボットカーにシリアル通信できるBluetoothモジュールを搭載し、PCやスマホにログを出力できるようにした。

----------

 

公園で試走をしたところ、位置情報の精度が低かったため、静止するポイントにばらつきはありましたが、ほぼ想定通りの走行ができました。

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

◆競技「QZSSスクランブル」のみのソースコード

＜参考にしたWebサイト＞

• 2個のモーターを制御できるArduino互換ボードのWebサイト
• Arduinoの入出力ピン
• NMEAデータ解析のサンプルコード
• GPSロボットカーの作成記録
  
• USBホストシールドの利用方法
• USBホストシールドのサンプルコード
• HMC5883L（3軸 デジタル・コンパス モジュール）のサンプルコード
  
• Bluetooth通信サンプルコード
• Tera Term（Windowsソフト）でデバイスとBluetoothでシリアル通信
• S2 Terminal for Bluetooth Free（Androidアプリ）

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

 

#include <cdcacm.h>

#include <usbhub.h>

#include "pgmstrings.h"

#include <SoftwareSerial.h>

#include <Wire.h> //I2C Arduino Library

#ifdef dobogusinclude

#include <spi4teensy3.h>

#endif

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

#define READBUFFERSIZE (256)

// 区切り記号

#define DELIMITER (",")

// π

#define pai 3.141592

// デジタルコンパス5883

#define HMC5883_WriteAddress 0x1E

#define HMC5883_ModeRegisterAddress 0x02

#define HMC5883_ContinuousModeCommand 0x00

#define HMC5883_DataOutputXMSBAddress  0x03

int regb=0x01;

int regbdata=0x40;

int outputData[6];

// bluetoothモジュールとのシリアル通信（デバック用）

SoftwareSerial mySerial(11, 12); // Rx, Tx

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

// 目的地緯度経度

float g_wpLong = 0;

float g_wpLat = 0;

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

char g_szNmea[READBUFFERSIZE] = "";

// ターゲットユニット保存バッファ

char g_szTarget[16];

// Arduinoボード上のキーNo.

int g_iKey=-1;

int adc_key_val[5] ={ 30, 150, 360, 535, 760 };

int NUM_KEYS = 5;

// ステータス

int g_iStatus = 0; // 0:スタートできる状態　1～3:n番目の目的地に向かって走る　4:初期状態or走行完了

// RMCを受診した回数

int g_iCounter = 0;

// ★待機フラグ（RMCを指定された回数受けたら走行制御する）

int g_iWait =1;

// ★車No.

char g_szCarNo[] = "02";

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

float X11 = 13946.5445;  // スタートポイント（経度）

float Y11 = 3537.1497;  // スタートポイント（緯度）

float X71 = 13946.53437;  // A7（経度）

float Y71 = 3537.1445;  // A7（緯度）

float X17 = 13946.53803;  // B7（経度）

float Y17 = 3537.1580;  // B7（緯度）

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

float Dax = (X71 - X11) / 6;

float Dbx = (X17 - X11) / 6;

float Day = (Y71 - Y11) / 6;

float Dby = (Y17 - Y11) / 6;

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

{

  // PWM DC control

  int i;

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

    pinMode(i, OUTPUT);

  

  // LED

  pinMode(13, OUTPUT);

  // bluetooth Serial

  mySerial.begin(9600);

  // I2C（デジタルコンパス）

  Wire.begin();

  // USBのイニシャライズ

  Usb.Init();

  delay( 200 );

}

void loop()

{

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

  int adc_key_in = analogRead(0);

  int iKey = get_key(adc_key_in);

  // キーが押された

  if(g_iKey != iKey && iKey != -1){

    g_iKey = iKey;

    g_iStatus = 0;

  }

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

  if(g_iStatus >= 4)

    return;

  

  // S2、又は、S3キーが押された以降はロボットカープログラムを実行（S3キーはテストモード）

  if(g_iKey == 1 || g_iKey == 2){

    ReadQZPOD();

  }

  // S1キーが押されたら走行を停止

  else if(g_iKey == 0)

    stop();

}

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

    // 0バイト以上受信した

    if( rcvd ) {

      char szReadBuffer[READBUFFERSIZE] = "";

      char szLineString[READBUFFERSIZE] = "";

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

      int len = strlen(g_szNmea);

      if(len > 1){

        strcpy(szReadBuffer, g_szNmea);

        memcpy(&szReadBuffer[len], buf, rcvd );

      }

      else{

        memcpy( szReadBuffer, buf, rcvd );

      }

      g_szNmea[0] = '\0';

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

        float fLat = 0.0;

        float fLong = 0.0;

        float fDeg = 0.0;

        // S3キーが押された場合はテストモード（QZQSMを無理やり入力）

        if(g_iKey == 2){

          strcpy(g_szNmea, "$QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08");

          g_iKey = 1;

        }

        // NMEAデータを解析する（$QZQSMと$GNRMCのみが対象）

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

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

        if(g_iStatus >= 1){

          // 目標地点が初期化されている場合は目標ポイントを算出する

          if(g_wpLat == 0){

            char c1[2], c2[2];

            int i0 = 0;

            int Na = 0;

            int Nb = 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);

      

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

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

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

          }

          // 受信データが$GNRMCだった

          else if(i == 1){

            // ★$GNRMCを指定回もしくはg_iWaitで指定された回数受信したら車体の制御を実行

            if(g_iCounter >= g_iWait){

              // 測位結果に基づいて車体を制御

              ControlRobotCar(fLat, fLong, fDeg);

              g_iCounter = 1;

              mySerial.println(fLat);

              mySerial.println(fLong);

              mySerial.println(fDeg);

            }

            else

              g_iCounter++;

            mySerial.println(g_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 = 1;

 

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

  while( 1 )

  {

    char c = szReadBuffer[iIndexChar];

    

    // 区切り文字の場合

    if( '$' == c){// && iIndexChar >= 1){

      char buf[ciLineStringSize];

      

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

      memcpy(szLineString, szReadBuffer, iIndexChar-1);

      szLineString[iIndexChar-1] = '\0';

      

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

      memcpy(buf, &szReadBuffer[iIndexChar], ciLineStringSize - iIndexChar);

      buf[ciLineStringSize - iIndexChar] = '\0';

      // szReadBufferにbufをコピーする

      memcpy(szReadBuffer, buf, ciLineStringSize);

      

      return 1;

    }

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

    else{

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

      int ibuf = strlen(szReadBuffer);

      if( ibuf - 1 <= iIndexChar ){

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

        memcpy( szLineString, szReadBuffer, ibuf);

        szLineString[ibuf] = '\0';

        szReadBuffer[0] = '\0';

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

  // 初めて対象となる$QZQSMを受信した場合

  // Sample $QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08

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

    mySerial.println( szLineString );

    //  車No.を読み取る

    c0[0] = szLineString[18];

    c0[1] = szLineString[19];

    c0[2] = '\0';

   

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

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

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

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

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

      g_szTarget[12] = '\0';

        

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

      g_iStatus = 1;

        

      // LEDを点灯する

      digitalWrite(13, HIGH);

      delay(3000);

      digitalWrite(13, LOW);

    }

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

    else{

      stop();

      g_iStatus = 0;

      g_wpLat = 0;

      g_wpLong = 0;

      return 0;

    }

    return 2;

  }

  // $GPRMCの場合

  // Sample $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 ) ){

    mySerial.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)

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

    if( NULL == pszLong )

      return 0;

    // 文字列を数値に変換

    rfLat = atof(pszLat);

    rfLong = atof(pszLong);

    

    // 車体の向きをデジタルコンパスより取得

    GetAngle(rfDeg);

    

    return 1;    

  }

  

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

  return 0;

}

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

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

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

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

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

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

{

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

  float jLong = (g_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((g_wpLong - rbLong) / (g_wpLat - rbLat));

  if((g_wpLong - rbLong) > 0 && (g_wpLat - rbLat) < 0)

    r_wp = pai + r_wp;

  else if((g_wpLong - rbLong) < 0 && (g_wpLat -rbLat) < 0)

    r_wp = pai + r_wp;

  else if((g_wpLong - rbLong) < 0 && (g_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 / 8){ // ★閾値は要変更

    turn_R(255,255);

  }

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

  else if(d_dev < (-1) * (pai / 8)){ // ★閾値は要変更

    turn_L(255,255);

  }

  else{

    advance(255,255);

  }

  return 1;

}

// デジタルコンパスから車体の向きを取得（北からの向き）

void GetAngle(float& angle) {

 

    int i,x,y,z;

    Wire.beginTransmission(HMC5883_WriteAddress);

    Wire.write(regb);

    Wire.write(regbdata);

    Wire.endTransmission();

 

//    delay(1000);

    Wire.beginTransmission(HMC5883_WriteAddress); //Initiate a transmission with HMC5883 (Write address).

    Wire.write(HMC5883_ModeRegisterAddress);       //Place the Mode Register Address in send-buffer.

    Wire.write(HMC5883_ContinuousModeCommand);     //Place the command for Continuous operation Mode in send-buffer.

    Wire.endTransmission();                       //Send the send-buffer to HMC5883 and end the I2C transmission.

//    delay(100);

 

 

    Wire.beginTransmission(HMC5883_WriteAddress);  //Initiate a transmission with HMC5883 (Write address).

    Wire.requestFrom(HMC5883_WriteAddress,6);      //Request 6 bytes of data from the address specified.

 

//    delay(500);

 

     //Read the value of magnetic components X,Y and Z

 

    if(6 <= Wire.available()) // If the number of bytes available for reading be <=6.

    {

        for(i=0;i<6;i++)

        {

            outputData[i]=Wire.read();  //Store the data in outputData buffer

        }

    }

 

    x=outputData[0] << 8 | outputData[1]; //Combine MSB and LSB of X Data output register

    z=outputData[2] << 8 | outputData[3]; //Combine MSB and LSB of Z Data output register

    y=outputData[4] << 8 | outputData[5]; //Combine MSB and LSB of Y Data output register

 

    // offset

    x = x+18;

    y = y+266;

    // angle in degrees

    angle= atan2((double)y,(double)x) * (180 / 3.14159265) +180;

    // ロボットカーの向きと、センサーの取り付け方向の関係から角度を変換

    if(angle >= 0 && angle < 90 )

        angle = angle + 270;

    else

        angle = angle - 90;

 

    //Print the approximate direction

    mySerial.print(angle,2);

    mySerial.print(" Deg / ");

    

    if((angle < 22.5) || (angle > 337.5 ))

        mySerial.println("North");

    if((angle > 22.5) && (angle < 67.5 ))

        mySerial.println("North-East");

    if((angle > 67.5) && (angle < 112.5 ))

        mySerial.println("East");

    if((angle > 112.5) && (angle < 157.5 ))

        mySerial.println("South-East");

    if((angle > 157.5) && (angle < 202.5 ))

        mySerial.println("South");

    if((angle > 202.5) && (angle < 247.5 ))

        mySerial.println("South-West");

    if((angle > 247.5) && (angle < 292.5 ))

        mySerial.println("West");

    if((angle > 292.5) && (angle < 337.5 ))

        mySerial.println("North-West"); 

    

//    delay(100);

}

// 止まる

void stop(void)

{

  digitalWrite(E1,LOW);

  digitalWrite(E2,LOW);

//  mySerial.println("stop");

}

// 前進する

void advance(char a,char b)

{

  digitalWrite(E1,LOW);

  analogWrite (E2,b);

  digitalWrite(M2,HIGH);

  mySerial.println("advance");

}

// 後進する

void back_off (char a,char b)

{

  digitalWrite(E1,LOW);

  analogWrite (E2,b);

  digitalWrite(M2,LOW);

  mySerial.println("back");

}

// 左に曲がる

void turn_L (char a,char b)

{

  analogWrite (E1,a);

  digitalWrite(M1,HIGH);

  analogWrite (E2,b);

  digitalWrite(M2,HIGH);

  mySerial.println("turn_L");

}

// 右に曲がる

void turn_R (char a,char b)

{

  analogWrite (E1,a);

  digitalWrite(M1,LOW);

  analogWrite (E2,b);

  digitalWrite(M2,HIGH);

  mySerial.println("turn_R");

}

// Arduinoボード上のキーが押されたらキーNo.を取得する

int get_key(unsigned int input)

{   

  int k;

  for (k = 0; k < NUM_KEYS; k++)

  {

    if (input < adc_key_val[k])

    {  

      return k;  

    }

  }

  // ボタンは押されなかった

  if (k >= NUM_KEYS)

    k = -1;

  return k;

}