卓上ロボット掃除機の改造 (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);してはだめですね。たまたま、動いていたんですね。
修正しました。