GPS・QZSSロボットカーコンテスト2015の競技「QZSSスクランブル」で準優勝した「ロボット・インフィニティ2015」の機体とソースコードの情報を公開します。

GPS・QZSSロボットカーコンテスト2015

http://robot-car.jimdo.com/

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

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

＜参考にしたWebサイト＞

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

【情報処理用Arduino】

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

#include <cdcacm.h>

#include <usb.h>

#include <SoftwareSerial.h>

#include <Wire.h>

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

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

// 2015/10/19オフセット実施

int offset_x=-85;

int offset_y=150;

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

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

// サーボ用シリアル通信

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

// 目的地の緯度経度格納用

long g_wpLong = 0;

long g_wpLat = 0;

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

char g_szNmea[READBUFFERSIZE] = "";

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

char g_szTarget[16];

// Arduinoボード上のキーNo.（キーは5つ）

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走行完了

// $GPRMCを受診した回数

int g_iCounter = 0;

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

int g_iWait =1;

// ★車No.

char g_szCarNo[] = "01";

// ★基準点（東京海洋大学2015）

long X11 = 139476272;  // スタートポイント（A1B1）の経度

long Y11 = 35399441;  // スタートポイント（A1B1）の緯度

long X71 = 139476357;  // A7B1の経度

long Y71 = 35399372;  // A7B1の緯度

long X17 = 139476188;  // A1B7の経度

long Y17 = 35399372;  // A1B7の緯度

// ★目的地到着の精度

long g_iAccuracy = 1;

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

long Dax = (X71 - X11) / 6;

long Dbx = (X17 - X11) / 6;

long Day = (Y71 - Y11) / 6;

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

{

  // LED PINの指定

  pinMode(13, OUTPUT);

  // Bluetooth用Serial通信の初期化

  BTSerial.begin(9600);

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

        long fLat = 0;

        long fLong = 0;

        double fDeg = 0;

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

        if(g_iKey ==2){

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

          g_iKey = 1;

        }

        // NMEAデータを解析する

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

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

        if(g_iStatus >= 1){

          // 目標地点が0の場合は目標ポイントを算出する

          if(g_wpLat == 0){

            char c1[2], c2[2];

            int i0 = 0;

            int Na = 0;

            int Nb = 0;

            // これから目指すターゲットユニットの番号をg_szTargetから抜き出す

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

          }

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

          if(i == 1){

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

            if(g_iCounter >= g_iWait){

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

              ControlRobotCar(fLat, fLong, fDeg);

              g_iCounter = 1;

              // 緯度・経度

              BTSerial.print("Lat:");

              BTSerial.println(fLat);

              BTSerial.print("Long:");

              BTSerial.println(fLong);

            }

            else

              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];// = {'\0'};

      // 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: 第1ターゲットの場所（5,7）

// 0602: 第2ターゲットの場所（6,2)

// 0205: 第3ターゲットの場所（2,5）

// 

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

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

{

  char c0[3];

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

  // $QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08

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

    BTSerial.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の場合

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

    BTSerial.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;

    // double型だと小数点以下4桁目の精度が悪いのでlong型で計算を行う（20150908)

    char szBuf[16], szBuf1[16];

    long iBuf, iBuf2;

    // 緯度

    strcpy(szBuf, pszLat);

    memcpy( szBuf1, &szBuf[5], 4);

    szBuf1[4]='\0';

    iBuf2 = atol(szBuf1);

    memcpy( szBuf1, szBuf, 4);

    szBuf1[4]='\0';

    iBuf = atol(szBuf1);

    iBuf = iBuf*10000 + iBuf2;    

    rfLat = iBuf;

    // 経度

    strcpy(szBuf, pszLong);

    memcpy( szBuf1, &szBuf[6], 4);

    szBuf1[4]='\0';

    iBuf2 = atol(szBuf1);

    memcpy( szBuf1, szBuf, 5);

    szBuf1[5]='\0';

    iBuf = atol(szBuf1);

    iBuf = iBuf*10000 + iBuf2;    

    rfLong = iBuf;

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

    GetAngle(rfDeg);

    return 1;    

  }

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

  return 0;

}

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

// long rbfLat;    ロボットカーの緯度

// long rbLong;    ロボットカーの経度

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

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

int ControlRobotCar(long& rbLat, long& rbLong, double& d_dir)

{

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

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

  double r_dir = (pai / 180) * d_dir;  // ロボットカーの進行方向のラジアン値

  double r_wp;  // ターゲットユニットの北からの方位

  double d_dev;  // 方位偏差

  long acc = g_iAccuracy*g_iAccuracy*(Dax*Dax + Dbx*Dbx); // 許容する範囲の単位

  long pos = (jLat*jLat + jLong*jLong); // 目標地点から現在地までの距離

  long div1 = 2*acc - pos;

  long div0 = acc - pos; //g_iAccuracy*g_iAccuracy*(Dax*Dax + Dbx*Dbx) - (jLat*jLat + jLong*jLong);

  BTSerial.print("div1:");

  BTSerial.println(div1);

  // ターゲットユニットに近づいた（ユニットサイズのルート2*iAccuracy倍以内にいる）

  if(div1 >= 0){

    // ゆっくり走る

    prudence();

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

    if(div0 >= 0){

      // 目的地に着いた

      destination();

      // ステータスの値を一つ上げる（次のターゲットを目指す）

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

      }

    }

    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;

  // 方位偏差の計算

  d_dev = r_wp - r_dir;

  if(d_dev <= -pai)

    d_dev = 2 * pai + d_dev;

  // ターゲットユニットが進行方向より右側の場合

  if(d_dev > pai / 6){ // ★閾値は要変更

    turn_R();

  }

  // ターゲットユニットが進行方向より左側の場合

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

    turn_L();

  }

  else{

    advance();

  }

  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;

    // データをログ出力する

    BTSerial.print(angle,2);

    BTSerial.print(" Deg / ");

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

        BTSerial.println("North");

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

        BTSerial.println("North-East");

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

        BTSerial.println("East");

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

        BTSerial.println("South-East");

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

        BTSerial.println("South");

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

        BTSerial.println("South-West");

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

        BTSerial.println("West");

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

        BTSerial.println("North-West"); 

}

// 止まる

void stop(void)

{

  ServoSerial.print("s");

  delay(50);

  BTSerial.println("stop");

}

// 前進する

void advance(void)

{

  ServoSerial.print("a");

  delay(50);

  BTSerial.println("advance");

}

// ゆっくり前進する

void prudence(void)

{

  ServoSerial.print("p");

  delay(50);

  BTSerial.println("prudence");

}

// 右に曲がる

void turn_R(void)

{

  ServoSerial.print("r");

  delay(50);

  BTSerial.println("turn_R");

}

// 左に曲がる

void turn_L(void)

{

  ServoSerial.print("l");

  delay(50);

  BTSerial.println("turn_L");

}

// 目的地に着いた

void destination(void)

{

  ServoSerial.print("d");

  delay(50);

  BTSerial.println("get_to_destination");

}

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

}

【機体制御用Arduino】

#include <Servo.h>

// 速度

#define add 80 //★

#define pru 85 //★

#define stp 100 //★

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

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

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

        RCmotor.write(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(30);

        RCmotor.write(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(90);

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

        RCmotor.write(stp);

        servo0.write(45);

        servo1.write(0);

        servo2.write(90);

        a_deg = 0;

        r_deg = 0;

        l_deg = 0;

        break;

    }

  }

}