アプリコンテスト機体情報

2016年7月30日掲載　測位航法学会

2016年9月25日修正　測位航法学会

2016年10月17日修正　測位航法学会

2016年10月22日修正　測位航法学会

・GPS/QZSS、GLONASS、BeiDouを同時並行受信

・測位精度は衛星配置にもよるが、1～2mが出る見込み。

QZSSスクランブル アプリコンテストで使用するロボットカーの動作を動画撮影しました。「S2 Terminal for Bluetooth」というAndroidアプリを使用して操作しています。

撮影時にロボットカーコンテストの会場の草が深かったため、アスファルトにて走行確認しました。

#include <SoftwareSerial.h>

#include <Wire.h>

#include <cdcacm.h>

#include <usb.h>

#define READBUFFERSIZE  (256)

#define DELIMITER (",")

// Bluetoothモジュール（シリアル通信・ログ出し用）

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

// サブArduino（ラジコンカー制御用Arduino）との通信用（シリアル通信）

SoftwareSerial SubArduinoSerial(8, 7); // Rx, Tx

// グローバル変数

int  g_iRCStatus = 0;

// デジタルコンパス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];

// ★デジタルコンパスは中心値がずれるので定期的にオフセットの見直しが必要

// 2016/10/22明治丸前でオフセット実施

int offset_x=-225;

int offset_y=-225;

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;

ACMAsyncOper  AsyncOper;

ACM           Acm(&Usb, &AsyncOper);

void setup()

{

  BTSerial.begin(9600); // Bluetooth通信

  // I2C通信の初期化

  Wire.begin(); // デジタルコンパス

  // LED PINの指定

  pinMode(13, OUTPUT);

  // USBのイニシャライズ

  Usb.Init();

}

void loop(void) 

{

  if (BTSerial.available()){

    char val = BTSerial.read();

    if(val != -1)

    {

      if(g_iRCStatus == 0){

        if(val == 'g'){

          BTSerial.println(val);

          BTSerial.end(); // サブボードへシリアル通信でコマンドを送るためBluetooth用のシリアルポートをいったん閉じる

          ready_to_go();

          g_iRCStatus = 1;

          BTSerial.begin(9600); // Bluetoothのシリアルポートをいったん閉じたのでまた開く

        }

      }

      else{

        BTSerial.println(val);

        BTSerial.end(); // サブボードへシリアル通信でコマンドを送るためBluetooth用のシリアルポートをいったん閉じる

        switch(val)

        {

          case 's':

            stop();

            break;

          case 'a':

            advance();

            break;

          case 'p':

            prudence();

            break;

          case 'R':

            turn_R();

            break;

          case 'r':

            turn_r();

            break;

          case 'L':

            turn_L();

            break;

          case 'l':

            turn_l();

            break;

          case '1':

          case '2':

            get_to_destination();

            break;

          case '3':

            get_to_destination();

            g_iRCStatus = 0;

            break;

          case 'e':

            emergency();

            g_iRCStatus = 0;

            break;

          default:

            stop();

            break;

        }

        BTSerial.begin(9600); // Bluetoothのシリアルポートをいったん閉じたのでまた開く

      }

    }

//    else

//      return;

  }

  if(g_iRCStatus != 0){

    // USBからNMEAデータを読み取る

    char szBuf[256];

    ReadData(szBuf);

    float fLat = 0.0;

    float fLong = 0.0;

    // NMEAデータの解析

    if( !AnalyzeLineString( szBuf, fLat, fLong ) )

    {

      return;

    }

    // 緯度、経度を読み取れた

    // 進行方向をデジタルコンパスより取得

    double fDeg;

    GetAngle(fDeg);

    // float to string

    char szLat[16];

    char szLong[16];

    char szDeg[16];

    dtostrf(fLat, 9, 4, szLat);

    dtostrf(fLong, 10, 4, szLong);

    dtostrf(fDeg, 5, 2, szDeg);

    // BT出力

    sprintf(szBuf, "Lat:%s\r\nLon:%s\r\nDeg:%s", szLat, szLong, szDeg);

    BTSerial.println(szBuf);

  }

}

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

void ReadData(char szLineString[])

{

  Usb.Task();

  if(Acm.isReady()) {

    uint8_t rcode;

    uint8_t uBuf[128];

    uint16_t rcvd = 128;

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

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

    if( rcvd ) {    // 受信した

      char szReadBuffer[READBUFFERSIZE] = "";

      memcpy( szReadBuffer, uBuf, rcvd );

      memcpy( szLineString, szReadBuffer, 128 );

    }

  }

}

// NMEAデータの解析

// $GP(N)RMCの場合、引数変数に、緯度、経度を入れ、戻り値 1 を返す。

// $GP(N)RMC以外の場合、戻り値は 0 を返す。

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

{

  rfLat = 0.0;

  rfLong = 0.0;

  // $GP(N)RMC

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

  {

    return 0;

  }

  // $GPRMC,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)

  if( NULL == pszLong )

  {

    return 0;

  }

  rfLat = atof(pszLat);

  rfLong = atof(pszLong);

  return 1;

}

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

void GetAngle(double& angle) {

  int i,x,y,z;

  Wire.beginTransmission(HMC5883_WriteAddress);

  Wire.write(regb);

  Wire.write(regbdata);

  Wire.endTransmission();

  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.

  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.

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

  // 時間が経つと中心値がずれるのでオフセットする

  x = x + offset_x;

  y = y + offset_y;

  // 方位の出力値

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

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

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

    angle = angle + 270;

  else

    angle = angle - 90;

}

// スタートの準備

void ready_to_go(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("d");

  SubArduinoSerial.end();

  stop();

  // LEDを点灯する

  digitalWrite(13, HIGH);

  delay(1000);

  digitalWrite(13, LOW);

}

// 止まる

void stop(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("s");

  SubArduinoSerial.end();

}

// 前進する

void advance(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("a");

  SubArduinoSerial.end();

}

// ゆっくり前進する

void prudence(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("p");

  SubArduinoSerial.end();

}

// 大きく右に曲がる

void turn_R(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("R");

  SubArduinoSerial.end();

}

// 少し右に曲がる

void turn_r(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("r");

  SubArduinoSerial.end();

}

// 左に曲がる

void turn_L(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("L");

  SubArduinoSerial.end();

}

// 少し左に曲がる

void turn_l(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("l");

  SubArduinoSerial.end();

}

// 目的地に着いた

void get_to_destination(void)

{

  SubArduinoSerial.begin(9600);

  SubArduinoSerial.print("d");

  SubArduinoSerial.end();

  // LEDを点灯する

  digitalWrite(13, HIGH);

  delay(1000);

  digitalWrite(13, LOW);

}

// 緊急停止

void emergency(void)

{

  stop();

  // LEDを点灯する

  digitalWrite(13, HIGH);

  delay(200);

  digitalWrite(13, LOW);

  delay(200);

  digitalWrite(13, HIGH);

  delay(200);

  digitalWrite(13, LOW);

  delay(200);

  digitalWrite(13, HIGH);

  delay(200);

  digitalWrite(13, LOW);

  delay(200);

  digitalWrite(13, HIGH);

  delay(200);

  digitalWrite(13, LOW);

  delay(200);

  digitalWrite(13, HIGH);

  delay(200);

  digitalWrite(13, LOW);

}

#include <Servo.h>

// 速度　2016/10/15明治丸前で調整実施

#define add 76 //★

#define pru 79 //★

#define stp 100 //★

// 方向　2016/10/15明治丸前で調整実施

#define Straight 90 //★

#define Turn_R 150 //★

#define Turn_r 110 //★

#define Turn_L 30 //★

#define Turn_l 70 //★

Servo RCservo;

Servo RCmotor;

Servo servo0;

Servo servo1;

Servo servo2;

int a_deg=0;  // 首の向き

int r_deg=0;  // 右腕の向き

int l_deg=0;  // 左腕の向き

void setup()

{

  Serial.begin(9600);

  Serial1.begin(9600);

  // 制御信号を送る出力ピンの設定

  RCservo.attach(10); // ラジコンステアリング

  RCmotor.attach(11); // ラジコンモーター

  servo0.attach(19); // 頭

  servo1.attach(20); // 右手

  servo2.attach(21); // 左手

}

void loop()

{

  if (Serial1.available()) {

    char inByte;

    inByte = Serial1.read();

    Serial.println(inByte);

    switch(inByte)

    {

      // 前進

      case 'a':

        RCservo.write(Straight);

        RCmotor.write(add);

        if(r_deg == 0)

        {

          servo0.write(90);

          servo1.write(90);

          servo2.write(90);

          a_deg = 1;

          r_deg = 1;

          l_deg = 0;

        }

        else if(r_deg == 1)

        {

          servo0.write(0);

          servo1.write(0);

          servo2.write(0);

          a_deg = 0;

          r_deg = 0;

          l_deg = 1;

        }

        break;

      // ゆっくり進む

      case 'p':

        RCservo.write(Straight);

        RCmotor.write(pru);

        servo0.write(0);

        servo1.write(0);

        servo2.write(90);

        a_deg = 0;

        r_deg = 0;

        l_deg = 0;

        break;

      // 大きく右に曲がる

      case 'R':

        RCservo.write(Turn_R);

        RCmotor.write(add);//pru);

        servo0.write(0);

        servo2.write(90);

        a_deg = 0;

        l_deg = 0;

        if(r_deg == 0)

        {

          servo1.write(90);

          r_deg = 1;

        }

        else if(r_deg == 1)

        {

          servo1.write(0);

          r_deg = 0;

        }

        break;

      // 小さく右に曲がる

      case 'r':

        RCservo.write(Turn_r);

        RCmotor.write(add);//pru);

        servo0.write(0);

        servo2.write(90);

        a_deg = 0;

        l_deg = 0;

        if(r_deg == 0)

        {

          servo1.write(90);

          r_deg = 1;

        }

        else if(r_deg == 1)

        {

          servo1.write(0);

          r_deg = 0;

        }

        break;

      // 大きく左に曲がる        

      case 'L':

        RCservo.write(Turn_L);

        RCmotor.write(add);//pru);

        servo0.write(90);

        servo1.write(0);

        a_deg = 0;

        r_deg = 0;

        if(l_deg == 0)

        {

          servo2.write(0);

          l_deg = 1;

        }

        else if(l_deg == 1)

        {

          servo2.write(90);

          l_deg = 0;

        }

        break;

      // 左に曲がる        

      case 'l':

        RCservo.write(Turn_l);

        RCmotor.write(add);//pru);

        servo0.write(90);

        servo1.write(0);

        a_deg = 0;

        r_deg = 0;

        if(l_deg == 0)

        {

          servo2.write(0);

          l_deg = 1;

        }

        else if(l_deg == 1)

        {

          servo2.write(90);

          l_deg = 0;

        }

        break;

      // 目的地に着いた

      case 'd':

        RCservo.write(Straight);

        RCmotor.write(stp);

        servo0.write(90);

        servo1.write(90);

        servo2.write(0);

        delay(300);

        servo0.write(0);

        servo1.write(0);

        servo2.write(90);

        delay(300);

        servo0.write(90);

        servo1.write(90);

        servo2.write(0);

        delay(300);

        servo0.write(0);

        servo1.write(0);

        servo2.write(90);

        delay(300);

        servo0.write(90);

        servo1.write(90);

        servo2.write(0);

        delay(300);

        servo0.write(0);

        servo1.write(0);

        servo2.write(90);

        a_deg = 0;

        r_deg = 0;

        l_deg = 0;        

        break;

      // 上記以外⇒停止

      default:

        RCservo.write(Straight);

        RCmotor.write(stp);

        servo0.write(45);

        servo1.write(0);

        servo2.write(90);

        a_deg = 0;

        r_deg = 0;

        l_deg = 0;

        break;

    }

  }

}