ロボット本体

 何度も作り変え、最終的にはこのような形になりました。この1台で行います。 最終形態.JPG

 このロボットの動きはまず、本体正面に付けた超音波センサでコップを探し、カニの手のようなアームでコップを掴みます。

 掴んだ後にコップをひっくり返して移動をするので、ボールが落ちないように蓋をつけました。蓋もモーターで動きます。 持ち上げる.JPG

 コップを掴んだあとは、再びセンサで空のコップをさ探して近づき、カニの手でコップを持ち上げ傾けてボールを注ぐ、という動きでボールを移し替えます。 注ぐとき.JPG

 片面でこの動きができれば、もう片方でも同じプログラムを動かせば良いだろうと考えました。

 また、改良前の段階で、カニの手のアームを固定させるためにモニターの上にたくさん部品を載せてしまい、モニターが見えなくなるという事態が生じました。

 それだと作業効率が悪いので、このようにしてしっかり見えるように改良しました。 モニター.JPG

 また、少ない部品でしっかり固定をするように工夫をしました。これなら持ち上げても外れません。 組み立て.JPG

 また、サンプルのロボットのキャスターを真似してキャスターを取り付けました。これにより、旋回の動きがスムーズになりました。 キャスター.JPG

動きの説明

フィールド.png

 右側に腕があるので、後ろ向きでスタートします。まず、1の場所まで後退します。(1)

 1の場所で半時計回りに旋回し少し前進して2の場所まで行き(2)、60度の範囲でコップを探します。(3)

 コップの位置と距離を確認して、10センチ手前まで進み、反時計周りに旋回します。(4)

 スレーブに「コップを掴む」というプログラムを起動させます。(5)

 スレーブは、コップを掴んだあとに音を発します。この音を感知するまで待ちます。(6)

 コップを掴んだら、その場で反時計回りに旋回し、次は空のコップのある4の場所を目指します。(7)

 少し後退することで、正確にコップの位置を把握するために、ある程度の距離をとります。(8)

 そのあとは、最初と同様に、超音波センサでコップの位置と距離を測り、10センチ手前まで進みます。(9)

 スレーブにそそぐプログラムを送信し(10)、コップを傾ける前にふたをさせます。

 コップを持ち上げた状態で反時計回りに旋回し、ふたを開けます。(11)

 持ち上げた状態で再び旋回し(12)、腕をおろすプログラムを送信します。(13)

 ボールを移したので、次は元の位置にコップを戻す動作にいきます。

 カニの手でコップを倒さないように、少し後退し、反時計回りに旋回します。(14)

 この動作は、何度も試して、具体的な値を入れて動かしています。

 この動きでだいたい3の場所あたりに移動できるので、コップを離すプログラムを送信します。(15)

 そのあとは、反対側のフィールドに移動をして、(3)あたりからの動作をさせるプログラムになっています。※反対側に移動してからの動作は、うまくいきませんでした。

プログラム(マスター側)

  • 定義
    #define SPEED 70
    #define SPEED_SLOW 45
    #define CONN 1 //スレーブの接続番号

 2つのNXTをBluetoothで接続します。向かって右側がマスター、腕の付いている左側がスレーブです。

 スレーブ側には腕の、マスター側にはコップを探して近づくプログラムを転送しておきます。  すべての指示をマスターが送ります。  

  • 10センチ手前まで行くプログラム [#f8124b94]

 以下のように、先生からもらったサンプルのプログラムを参考に作りました。

const float diameter = 5.45; //タイヤの直径(cm)
const float track = 12; //タイヤのトレッド幅(cm)
const float pi = 3.1415; //円周率
long angle_save;

void fwdDist(float d) //距離dcm前進
{
 long angle;
 angle = d/(diameter*pi)*360.0; //角度を計算する
 RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 0, true, true);
}

void turnAng(long ang) //角度ang度の時計回りの旋回
{
 long angle;
 angle = track/diameter * ang;
 RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 100, true, true);
}

void ReturnAng(long ang) //角度ang度の半時計回りの旋回
{
 Wait(500);
 long angle;
 angle = track/diameter * ang;
 RotateMotorEx(OUT_BC, SPEED_SLOW, angle, -100, true, true);
} 

int searchDirection(long ang) //現在の方向を中心にang度の範囲で探し、障害物までの距離を返す
{
 long angle,tacho_min=0, tacho_corr;
 int d_min;
 d_min=600; //仮の最小値
 angle = (track/diameter)*ang; //旋回角度からタイヤの回転を計算
 turnAng(ang/2); //指定された角度の半分を旋回
 ResetTachoCount(OUT_BC); //角度計測をリセット

 OnFwdSync(OUT_BC,SPEED_SLOW-15,-100); //半時計回りに旋回
 while(MotorTachoCount(OUT_B)<=angle){
 if(SensorUS(S1)<d_min){
  d_min=SensorUS(S1);
  tacho_min=MotorTachoCount(OUT_B);
  }
 }
 angle_save=tacho_min;
 OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
 until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);

 Wait(14); //微調整
 Off(OUT_BC);Wait(500);
 return d_min;
}

 これは本体の上に付けた音のセンサで使用します。スレーブが動作を終えたあとに鳴らす音を感知して、その動作が終わったことを確認してから次の動作に移るためです。

void WAIT(){
 while(Sensor(S2)>99)//返事(音)を待つ
 {Wait(1);}
}

 スタートする時からカニの爪を開いたままコップの手前まで行きます。そのあと半時計回りに旋回してコップをキャッチします。この一連の動作を以下のように「SearchCup」という命令にしました。  

void SearchCup(int a, int b)//a=何cm手前で停止、b=行って何度回転する     
{int d = searchDirection(60);
if (d > a) {
  fwdDist(d-a);
 }
 Wait(100);
 ReturnAng(b); //反時計周りにb度旋回
 Off(OUT_BC);
}

 これは、逆の動作をさせるためのプログラムです。コップを持って行く過程を逆にたどれば、元々コップの置いてあった場所まで戻れると考えましたが、思っていたようには動かなかったので、使用せず他の方法を取りました。

void comeback(int d, int cm, int turn, int angle){
ReturnAng(angle);

fwdDist(-d);
turnAng(turn);
}
task main()
{
SetSensorLowspeed(S1); //端子1に超音波センサ
SetSensorSound(S2); //端子2に音のセンサ
int ang1,ang2,d1,d2,d;

fwdDist(-40.0); //50センチ後退(1)
Wait(500);

ReturnAng(70); //反時計周りに70度旋回(2)
Wait(500);

fwdDist(25.0); //25センチ前進 (3)
Wait(500);

/*ボールの入ったコップの方向を探した後、近づいて10センチ手前で停止*/(4)
SearchCup(8,70);
ang1=angle_save;
d=d1;

until(BluetoothStatus(CONN)==NO_ERR); //接続できるまで待つ

/*コップに近づいた後スレーブコップをつかむプログラムを送信*/(5)
RemoteStartProgram(CONN,"hold_sound.rxe");

WAIT();//スレーブが音を発するまで待つ(6)
ReturnAng(130); //反時計周り130度旋回(7)
Wait(500);

fwdDist(-15); //15cm後退(8)

/*空のコップの方向を探した後、近づいて10センチ手前で停止*/(9)
SearchCup(11,0);
ang2=angle_save;
d=d2;

/*空のコップに近づいた後スレーブにボールを注ぐプログラムを送信*/(10)
RemoteStartProgram(CONN,"pour_up.rxe");
Wait(5000);

ReturnAng(50); //反時計回りに50度旋回
RemoteStartProgram(CONN,"pour_open.rxe");(11)
Wait(3000);

turnAng(-40);(12)

RemoteStartProgram(CONN,"pour_down.rxe");(13)
Wait(3000);

fwdDist(-20);
Wait(500);

ReturnAng(120);(14)
Wait(500);

/*スレーブにコップを離すプログラムを送信*/(15)
RemoteStartProgram(CONN,"release_sound.rxe");
WAIT();

turnAng(90);

fwdDist(-25);
Wait(500);

turnAng(-70);
Wait(500);

fwdDist(30);
SearchCup(8,70);

/*ボールの入ったコップの方向を探した後、近づいて10センチ手前で停止*/
 SearchCup(8,70);
 ang1=angle_save;
 d=d1;

until(BluetoothStatus(CONN)==NO_ERR); //接続できるまで待つ

/*コップに近づいた後スレーブコップをつかむプログラムを送信*/
RemoteStartProgram(CONN,"hold_sound.rxe");

WAIT();//スレーブが音を発するまで待つ
ReturnAng(130); //反時計周りに360度旋回
Wait(500);

fwdDist(-15); //10cm後退

/*空のコップの方向を探した後、近づいて10センチ手前で停止*/
SearchCup(11,0);
ang2=angle_save;
d=d2;

/*空のコップに近づいた後スレーブにボールを注ぐプログラムを送信*/
RemoteStartProgram(CONN,"pour_up.rxe");
Wait(5000);

ReturnAng(50);

RemoteStartProgram(CONN,"pour_open.rxe");
Wait(3000);

turnAng(-40);

RemoteStartProgram(CONN,"pour_down.rxe");
Wait(3000);

fwdDist(-20);
Wait(500);

ReturnAng(90);
Wait(500);

/*スレーブにコップを離すプログラムを送信*/
RemoteStartProgram(CONN,"release_sound.rxe");
WAIT();
}

プログラム(スレーブ側)

 コップを掴むプログラム

/* モーターAがハサミ、モーターBが腕 */
#define HOLD OnFwdReg(OUT_A,-40,OUT_REGMODE_IDLE);  //コップを挟む
task main()
{
 HOLD;  //掴む
 Wait(500);  //ちょっと待つ
 Off(OUT_A);  //モーターAを切る
 Wait(500);
 PlaySound(SOUND_UP);
 Wait(100);
 PlaySound(SOUND_UP);
 Wait(100);
}

 コップを離すプログラム

/* モーターAがハサミ、モーターBが腕 */
#define RELEASE RotateMotor(OUT_A,50,50);  //コップをはなす
task main()
{
 ResetTachoCount(OUT_AB);  //モーターABの回転角をリセット
 RELEASE;  //コップを放す
 Wait(300);
 PlaySound(SOUND_DOWN);
 Wait(100);
 PlaySound(SOUND_DOWN);
 Wait(100);
}

ふりかえり

よかった点

  • 超音波センサを丁度良い位置に設置できたので、スタートの位置が多少ずれても、コップの位置を正確に判断することができ、確実につかんで運ぶことができました。
  • キャスター付きに改良したので旋回が楽になり、少ないパワーでも旋回できるようになり、遅いスピードで旋回できたので、コップを掴む際に倒してしまう失敗がなくなりました。
  • スレーブからマスターにメッセージが送れなかったが、音を使って認識するという代用がとてもよかったです。

悪かった点

  • プログラム名を指定して、スレーブ側のプログラムを起動させることは問題なくできたが、スレーブからマスターにメッセージを送ることができなかった。
  • 本体の組み立て時間をかけてしまい、何度も本体を作り変えてしまったので、もっと早くに本体を決めてしまえば、もう片方の面へ行く動作も正確にプログラムできたと思った。
  • 超音波センサのみに頼っていたので、全体のどの位置にいるかを全く把握できなかったので、光センサを使えたらもっと確実にできると思った。
  • 電池が古くなってくるとモーターを回すパワーが足りず、旋回できなくなり、動きが止まってしまうことがあった。
  • 毎回正確にコップの前まで行くことができた訳ではなかったので、2度に分けて探したら解決しそうです。

添付ファイル: fileフィールド.png 98件 [詳細] file注ぐとき.JPG 111件 [詳細] file組み立て.JPG 154件 [詳細] file持ち上げる.JPG 145件 [詳細] file最終形態.JPG 94件 [詳細] fileモニター.JPG 109件 [詳細] fileコックピット.JPG 59件 [詳細] fileキャスター.JPG 129件 [詳細] fileつかむ.JPG 50件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2015-02-09 (月) 21:28:19