卓上ロボット掃除機の改造 (11)

ロボット用のPICのファームを修正しました。Androidとネゴシエーションを行う処理を変更して、Androidがポートを開くまで待ち続けるようにしました。
この修正にともなって、ロボットの制御のプロトコルに接続待ち: Cという命令を追加しました。

ロボットの制御コマンド

命令は7つになりました。

• 前進: F
• 後退: B
• 右回転: R
• 左回転: L
• 停止: S
• 接続: C
• 終了: Q

2バイトのプロトコルで、AndroidからのFx, Bx, Rx, Lx, Sx, Cx, Qx命令を待っています。xは0〜255段階のDCモータに掛かる電圧を意味します。よって、SとCとQ命令では、xは無視されます。

プログラム

ポートアクセスをするところは、下のように無限ループにしました。全体のソースは、githubにアップしています。ここです。

//***********************************************
// tcp Forward
//***********************************************
Connection* tcpForward(const char* port)
{
char ac[128];
 strcpy( ac, "tcp:" );
 strcat( ac, port );
 Connection *con = ADB_addConnection( ac, false);
 AdbDataReceiveFlg = false;
 int i;
 while(true){
   for(i=0;i<6;i++){
     ADB_poll(con);
     if(AdbDataReceiveFlg == true){ break; }
     LATBbits.LATB2 = 1;
     LATBbits.LATB15 = 1;
     DelayMs(20);
     LATBbits.LATB2 = 0;
     LATBbits.LATB15 = 0;
     DelayMs(100);
   }
   if(AdbDataReceiveFlg==true){ break; }
   ADB_delConnection(con);
   DelayMs(50);
   con = ADB_addConnection( ac, false);
 }
 return con;
}

あとは、ソースの途中ですが、QコマンドとCコマンドの処理のところを書いておきます。

      case 'Q':
        OC4R = 0;
        OC5R = 0;
        setMotorControl(0,MOTOR_STOP);
        //Straight Gear
        setServoAngle(0,160);
        //LED OFF
        LATBbits.LATB2 = 0;
        LATBbits.LATB15 = 0;
        ADB_delConnection(AdbC);
        DelayMs(250);
        ClrWdt();
        Sleep();
        break;
      case 'C':
        ConnectFlg = true;
        break;
      default:
        RecvData[0] = 'E';
        break;
    }
    ADB_write(AdbC, 1,RecvData);
    AdbDataReceiveFlg = false;
    if(ConnectFlg==true){
      ADB_delConnection(AdbC);
      DelayMs(250);
      //ReConnection
      AdbC = tcpForward("60101");
      AdbDataReceiveFlg = false;
      ConnectFlg = false;
    }
   }
   DelayMs(1);
 }

2012/3/11 修正

上のプログラムのcase 'C': がバグっていました。
ADB_delConnection(AdbC);とした後に、ADB_write(AdbC, 1,RecvData);してはだめですね。たまたま、動いていたんですね。
修正しました。