ロボット・インフィニティにLCDディスプレイを付けました。

 

USBホストシールドとPINが被っていたので、ワイヤーを使ってPINを入れ換えたら映るようになりました。

 

LCDに情報表示してプログラムをデバッグしたら、バグだらけということが分かりました。

 

本番はまともに動かなかった訳です。

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

＜参考にしたWebサイト＞

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

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

#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

#include <LiquidCrystal.h>

LiquidCrystal lcd( 8, 11, 12, 1, 2, 3);

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

#define READBUFFERSIZE (256)

// 区切り記号

#define DELIMITER (",")

// π

#define pai 3.141592

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

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

char g_szNmea[READBUFFERSIZE] = "";

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

char g_szTarget[16];

// キーNo.の保存用

int key=-1;

// ステータス

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

// ★車No.

char CarNo[] = "02";

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

int iCounter = 0;

// ★待機フラグ

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

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

//    ;

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

  // LCDを点灯する

  lcd.begin(16, 2);

  lcd.print("Robot Infinity!");

  delay( 200 );

}

void loop()

{

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

  int adc_key_in = analogRead(0);

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

  if(adc_key_in <360){ // S1:0 S2:143or144

    key = adc_key_in;

    // スタートできる状態

    g_iStatus = 0;

  }

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

  if(g_iStatus >= 4)

    return;

  

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

  if(key >30){

    ReadQZPOD();

  }

  // S1キーが押された以降は走行を停止

  else{

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

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

    if( rcvd ) {

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

        float fLat = 0.0;

        float fLong = 0.0;

        float fDeg = 0.0;

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

        if(key >150){

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

          key = 144;

        }

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

            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だったので走行の処理を行う

          else if(i == 1){

/*            // LDCに緯度経度を表示

            lcd.clear();

            lcd.setCursor(0, 0);

            lcd.print(fLat*10000);

            lcd.setCursor(0, 1);

            lcd.print(fLong*10000);*/

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

            if(iCounter >= iWait){

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

              ControlRobotCar(fLat, fLong, fDeg);

              iCounter = 1;

              // ★待機フラグ

              iWait = 5;

            }

            else

              iCounter++;

              

            // LCDにiCounterを表示

            lcd.setCursor(0, 1);

            lcd.print(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';

        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を受信した場合

  // $QZQSM,55,C6C407DE0207030703030101050706020205000000000000000000001BAD358*08

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

    Serial.println( szLineString );

    //  車No.を読み取る

    c0[0] = szLineString[18];

    c0[1] = szLineString[19];

    c0[2] = '\0';

   

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

    if(strcmp(c0, CarNo) == 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;

    }

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

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

    

    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;

  // LDCに緯度経度を表示

  float fBuf = d_dev*180/pai;

  lcd.clear();

  lcd.setCursor(0, 0);

  lcd.print(fBuf);

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

  if(d_dev > pai / 4 && d_dev <= pai / 2){ // ★閾値は要変更

    turn_R(255,255);

    advance(255,255);

    // ★待機フラグ

    iWait = 10;

  }

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

    turn_R(255,255);

    advance(255,255);

    // ★待機フラグ

    iWait = 15;

  }

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

  else if(d_dev < (-1) * (pai / 4) && d_dev >= (-1) * (pai / 2)){ // ★閾値は要変更

    turn_L(255,255);

    advance(255,255);

    // ★待機フラグ

    iWait = 10;

  }

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

    turn_L(255,255);

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

}