10/18（土）に東京海洋大学で開催された「GPS・QZSSロボットカーコンテスト2014」のQZSSスクランブルに出場しました。天気が良くとても気持ちの良い日でした。

16チームの個性豊かなロボットカーが集まりました。2種類の競技のうち、ロボット・インフィニティは「QZSSスクランブル」に出場しました。

ロボット・インフィニティの小さな車輪にとっては芝が深くてまっすぐ進むことができませんでした。それでも準天頂衛星「みちびき」からの災危通報を受けてスタートを切ることができ、準優勝することができました。

http://www.qzs.jp/events/201408robocar/result.html

11月15日（土）に日本科学未来館で開催されるG空間EXPO2014のデモ走行ではもう少し良い走りを見せたいです。

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

//追加

#define READBUFFERSIZE (256)

#define DELIMITER (",")

#define pai 3.141592

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;

int adc_key_in=-1;

int key=-1;

int iCounter = 0;

// 出走番号フラグ

int g_iFlag = 0;

/* 基準点の設定 */

float rpLat = 3539.93400;

float rpLong = 13947.61983;

float rpLatA = 3539.94383;

float rpLongB = 13947.63167;

float gdLat = (rpLatA - rpLat) / 6;

float gdLong = (rpLongB - rpLong) / 6;

/* 目的地 */

float g_wpLong = 0;

float g_wpLat = 0;

char CarNo[] = "05";

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

  pinMode(13, OUTPUT);  //we'll use the debug LED to output a heartbeat

  

//  Serial.begin( 115200 );

//  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection

//  Serial.println("Start");

  if (Usb.Init() == -1)

    ;

//      Serial.println("OSCOKIRQ failed to assert");

  delay( 200 );

}

void loop()

{

  adc_key_in = analogRead(0);

  

  if(adc_key_in == 143 || adc_key_in == 144 || adc_key_in == 0){

    key = adc_key_in;

  }

  

  // S2キーが押された

  if(key == 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;

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

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

        char szReadBuffer[READBUFFERSIZE];

        char szLineString[READBUFFERSIZE];

        memcpy( szReadBuffer, buf, rcvd );

          

        while(1){

          // QZPODからシリアル出力されたデータを読み取り、データを区切る

          if( !ReadLineString( szReadBuffer, rcvd, szLineString, READBUFFERSIZE ) ){ // 読み取り途中

            strcat( g_szNmea, szLineString );

            return;

          }

          if( strlen(szLineString) == 0)

            return;

              

          // 区切り完了

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

          strcat( g_szNmea, szLineString );

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

//          Serial.println(g_szNmea);

          float fLat = 0.0;

          float fLong = 0.0;

          float fDeg = 0.0;

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

          

//          Serial.println(i);

          if(g_iFlag == 1){          

            // $QZQSMを読み込んだ

            if(i == 2){

              g_wpLat = fLat;

              g_wpLong = fLong;

            }

            // $GNRMCを読み込んだ

            else if( i == 1){

              if( iCounter == 10){

                if(2 == ControlRobotCar(fLat, fLong, g_wpLat, g_wpLong, fDeg)){

                  // 目的地に着いた

                  int i = 0;

                  while(i < 20){

                    stop();

                    digitalWrite(13, HIGH);

                    delay(500);

                    digitalWrite(13, LOW);

                    delay(300);

                    i++;

                  }

                }

                iCounter = 1;

              }

              else

                iCounter++;

            }

          }

          g_szNmea[0] = '\0';

        }

      }

      delay(10);

    }

}

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

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

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

{

  int iIndexChar = 0;

  

  while( 1 )

  {

    char c = szReadBuffer[iIndexChar];

    

    if( '\r' == c){ // 終端

      char buf[ciLineStringSize];

      szReadBuffer[iIndexChar] = '\0';

      strcpy( szLineString, szReadBuffer);

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

        iIndexChar++;

      

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

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

      strcpy(szReadBuffer, buf);

      

      return 1;

    }

    else{ // 途中

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

        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以降がロボットの番号やメッシュデータと行き先を表しています。

// 最後の1BAD358*08は調整用のメッセージおよびパリティです。　

// 

// 解析例は、次の通りです。

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

{

  rfLat = 0.0;

  rfLong = 0.0;

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

  int i0 = 0;

  int iLat = 0;

  int iLong = 0;

  

  // $QZQSMを読み取る

  // $QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08

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

//    Serial.println( szLineString );

//  出走番号を読み取る

    c0[0] = szLineString[18];

    c0[1] = szLineString[19];

    c0[2] = '\0';

   

    // 指定された番号の場合

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

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

      if(g_iFlag == 0){

        g_iFlag = 1;

        digitalWrite(13, HIGH);

        delay(3000);

        digitalWrite(13, LOW);

        advance(255,255);

      }

    }

    else{

      stop();

      g_iFlag = 0;

      g_wpLat = 0;

      g_wpLong = 0;

      return 0;

    }

    

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

    if( g_wpLat == 0 ){

      c1[0] = szLineString[35];

      c1[1] = '\0';

      c2[0] = szLineString[37];

      c2[1] = '\0';

      iLat = atoi(c1);

      iLong = atoi(c2);

      

      rfLat = rpLat + gdLat * (iLat-1);

      rfLong = rpLong + gdLong * (iLong-1);

      return 2;

    }

    else

      return 0;

  }

  // $GPRMC以外

  if( 0 != strncmp( "$GNRMC", szLineString, 6 ) && 0 != strncmp( "$GPRMC", szLineString, 6 ) )

    return 0;

//  Serial.println( szLineString );

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

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

  

  // dddmm.mmmm → ddd.dddddd

/*  float temp, deg, min;

  temp = atof(pszLat);

  deg = (int)(temp/100);

  min = temp - deg * 100;

  rfLat = deg + min / 60;

  temp = atof(pszLong);

  deg = (int)(temp/100);

  min = temp - deg * 100;

  rfLong = deg + min / 60;

  temp = atof(pszDeg);

  rfDeg = (int)(temp);*/

  return 1;

}

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

// 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; // way point到達判定x値

  float jLong; // way point到達判定y値

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

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

  float d_dev; // 方位偏差

/* 目標地点に到達した場合のway pointの再設定 */

/* 目標地点到達判定計算 */

  jLat = (wpLat - rbLat);

  jLong = (wpLong - rbLong);

  if(jLat <= gdLat && jLat >= -gdLat && jLong <= gdLong && jLong >= -gdLong){

    stop();

    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 ){ // ウェイポイントが進行方向より右側の場合

    turn_L(255,255);

    delay(1000);

    advance(255,255);

  }

  else if(d_dev < (-1) * (pai / 4)){ // ウェイポイントが進行方向より左側の場合

    turn_R(255,255);

    delay(1000);

    advance(255,255);

  }

  else{

    advance(255,255);

  }

  return 1;

}

void stop(void)                    //Stop

{

  digitalWrite(E1,LOW);   

  digitalWrite(E2,LOW);      

}   

void advance(char a,char b)          //Move forward

{

  analogWrite (E1,a);      //PWM Speed Control

  digitalWrite(M1,HIGH);    

  analogWrite (E2,b);    

  digitalWrite(M2,HIGH);

}  

void back_off (char a,char b)          //Move backward

{

  analogWrite (E1,a);

  digitalWrite(M1,LOW);   

  analogWrite (E2,b);    

  digitalWrite(M2,LOW);

}

void turn_L (char a,char b)             //Turn Left

{

  analogWrite (E1,a);

  digitalWrite(M1,LOW);    

  analogWrite (E2,b);    

  digitalWrite(M2,HIGH);

}

void turn_R (char a,char b)             //Turn Right

{

  analogWrite (E1,a);

  digitalWrite(M1,HIGH);    

  analogWrite (E2,b);    

  digitalWrite(M2,LOW);

}