ホーム2017b/Member
以下は2017b/Mission3のからの引用です。
ピンポン玉と紙コップを別々に回収して所定の場所に運ぶ。
Aに置くロボットとDに置くロボットの2台作りました。基本構造は同じですが、超音波センサーのつけ方や明るさセンサーと接触センサーも有無に違いがあります。私はDに置く方を担当しました。
左の写真がロボットの全体を上から見た図、右の写真が斜め前から見た図です。
明るさセンサーと接触センサーを取り付けています。パーツの中でも最も重量のある電池ボックス(兼制御部)の位置を少しだけでも下げるために元は斜めに設置して合ったも配線の都合上、元の形とは上下を入れ替えています。のを、倒してなるべく水平に設置するよう改良しました。
左の写真がロボットの全体を上から見た図、右の写真が斜め前から見た図です。
こちらは光センサーと接触センサーを付けていません。詳しくは2017b/Member/Masa/Mission3や2017b/Member/Taka/Mission3を見てください。
超音波センサーと明るさセンサーがロボットの中心の延長線上に来るように作りました。
NXTは一つの本体につきモーターを3つまでしか接続できません、制作時点でそのうち2つを移動用に割り当てているためモーター一つでコップを掴む、持ち上げる、離すの動作をしなければなりません。そのため先生から聞いたアイデアを実現することにしました。以下はその概念図です。
モーターが回転した時にアームを閉じるか持ち上げるかのどちらかが動きます。その時負荷の低い方が動くので先にアームを閉じて、その後閉じ切ってからアームを持ち上げます。持ち上げる量はモーターの回転量に応じて変化するので、少しだけ持ち上げるということができます。
この画像の3番のようにアームを少し上げた状態にすることでピンポン玉を中に保持したまま紙コップを運べるようになります。
プログラムを簡略化し、開発しやすくするためにいくつか定数を定義しました。
以下はその抜粋です
#define Apower 37
#define Bpower 34 //直進するのに必要な出力
#define ttime 695 //90度曲がるのに必要な時間
#define gotime 689 //20cm進むのに必要な時間
#define SPEED 50
#define SPEED_SLOW 30 まず上二つが移動用モーターの出力です。数値を左右で変えてまっすぐ先進するようにしました。3つめは旋回時に90°回転するための待ち時間です。最後の二つはモータの回転角度を指定してより正確に前進、旋回する時のモーターの出力です。
またロボットの基礎データを固定された変数として定義しました。
const float diameter=5.45;//タイヤの直径 const float track=11.35;//タイヤのトレット幅 const float pi=3.1415;//円周率
定数と同様にいくつかサブ関数を定義しました。これにより可読性を高め、他人が見てもわかりやすくなります。
サブ関数の中には中にサブ関数を使用しているところがあるので分けて説明します。
sub go_for() { OnFwd(OUT_A,Apower); OnFwd(OUT_B,Bpower); } sub go_back() { OnFwd(OUT_A,-Apower); OnFwd(OUT_B,-Bpower); } sub turnL(float c) { OnFwd(OUT_A,Apower);OnFwd(OUT_B,-Bpower); Wait(c*ttime); Float(OUT_AB); //左に曲がる(c×90)度 } sub turnR(float c) { OnFwd(OUT_A,-Apower);OnFwd(OUT_B,Bpower); Wait(c*ttime); Float(OUT_AB); //右に曲がる(c×90)度 }
使用した変数
ここで上げたサブ関数はロボットを動かす基本動作です。先進、後進、左旋回、右旋回のためのサブ関数です。前進、後進は開始のみ、旋回は回転する角度を指定して旋回します。
int cyari(int lsw) { ResetTachoCount(OUT_C); SetSensorLight(S1); SetSensorMode(S1,SENSOR_MODE_RAW);//明るさセンサを生データに SetSensorTouch(S4); int i=1; int lsb=SENSOR_1; int lsb2=SENSOR_1-1; while (i==1){ go_for(); lsb2=SENSOR_1; NumOut(80,LCD_LINE4,lsb2); if (lsb>lsb2) { lsb=lsb2; } if (lsb+10<lsb2) { i=10; } if (lsb>lsw-150) { i=1; } } Off(OUT_AB); NumOut(80,LCD_LINE1,lsw); NumOut(80,LCD_LINE2,lsb); return lsb; }
使用した変数
黒線の明るさの最低値を自動取得するプログラムです。進みながら明るさを取得し比較し記録します。下図のように紙面の白色部分から動き始めて黒線を通過するように通過します。
実際のプログラムは黒線の明るさがわかった後は必要ないので、最低値から10以上大きい数値を検知したら終了するようになっています。最終的に紙面(白色)と黒線の明るさを画面に表示して、lsbの数値を返して終了です。
本来は黒線の明るさを取得するプログラムですが、特定の明るさになったら止まるという特性を利用して、今回はスタート時の位置調整の機能を担っています。
void fwdDist(float d) { long angle=d/(diameter*pi)*360.0; RotateMotorEx(OUT_AB,SPEED_SLOW,angle,0,true,true); } void turnAng(long ang) { long angle=track/diameter*ang; RotateMotorEx(OUT_AB,SPEED_SLOW,angle,100,true,true); }
先生からもらったプリントをそのまま参考にして精密に移動できるサブ関数を作りました。
sub arm(int c) { RotateMotor(OUT_C, 40,c); Off(OUT_C); }
アームのモーターが指定した角度だけ回転します。
ここまでで定義したものを使って次のようにtask mainを作りました。 task main () {
ResetTachoCount(OUT_C); SetSensorLight(S1); SetSensorMode(S1,SENSOR_MODE_RAW); SetSensorTouch(S4); int lsw=SENSOR_1; //位置調整 int lsb=cyari(lsw); while(SENSOR_4==0) {}//待機 fwdDist(15); //Q付近の紙コップまで行く Wait(15000); //その間でロボット2と干渉を避けるため待つ turnL(1.1); PlaySound(SOUND_UP); Wait(1000); fwdDist(30); Wait(1000); turnAng(40);//ロボットを紙コップに向ける PlaySound(SOUND_UP); turnAng(180); //反転して後ろのアームを紙コップに向ける Wait(1000); fwdDist(15); //紙コップと少し距離をとる Wait(1000); arm(550); //アームを開いて最大限にまで下げる。 Wait(1000); go_back(); Wait(2000); //近づいてアームの中に紙コップを入れる Off(OUT_AB); arm(-180); //アームを閉じる Wait(1000); arm(-45); //アームを少し持ち上げて割り箸を乗り越える余裕を作る Wait(1000); turnL(1.05); //旋回して割りばしの中に行く Wait(1000); arm(-200); //アームを上げてピンポン玉を割りばしの中に入れる fwdDist(35); //Y地点に向かう turnL(0.4); Wait(1000); arm(100); //紙コップを落とす PlaySound(SOUND_UP); SendRemoteNumber(1,MAILBOX1,1); //紙コップを運びおえたことをロボット2に知らせる fwdDist(20); //Y地点から立ち去る while(SENSOR_4==0) {} //終了
}
私が作ったロボットは紙コップを一つとって終了です。後述の理由により超音波センサーは使いませんでした。
事前の試走ではうまく動作しましたが、本番では旋回角度が正確でなく間違った方向に進んでしまいました。 右旋回は角度を指定して動けたのに対し、左旋回は時間によって制御するためずれができたと考えます。
基本点は三回目の挑戦で5点を獲得しました。
#define SPEED_SLOW 25 const float diameter = 5.45; //タイヤの直径(cm) const float track = 10.91; //タイヤのトレッド幅(cm) const float pi = 3.1415; //円周率
void fwdDist(float d) //距離 d cm 前進 { long angle = d/(diameter*pi)*360.0; //必要なタイヤの回転角度 RotateMotorEx(OUT_AB,SPEED_SLOW, angle,0,true,true); } void baDist(float d) //距離 d cm 後進 { long angle = d/(diameter*pi)*360.0; //必要なタイヤの回転角度 RotateMotorEx(OUT_AB,SPEED_SLOW, -angle,0,true,true); } void turnAng(long ang) //角度ang度の時計回りの旋回 { long angle = track/diameter * ang; //必要なタイヤの回転角度 RotateMotorEx(OUT_AB,SPEED_SLOW, angle,100,true,true); } void nturnAng(long ang) //角度ang度の時計回りの旋回 { long angle = track/diameter * ang; //必要なタイヤの回転角度 RotateMotorEx(OUT_AB,SPEED_SLOW, angle,0,true,true); } int searchDirection(long ang) //現在の方向を中心にang度の範囲で探し //障害物までの距離を返す { long tacho_min ; //もっとも近い距離を実現するタイヤの回転数 int d_min = 300 ; //もっとも近い距離の仮の最小値
long angle = (track/diameter)*ang; //旋回角度からタイヤの回転を計算 turnAng(ang/2); //指定された角度の半分を旋回 ResetTachoCount(OUT_AB); //角度計測をリセット
OnFwdSync(OUT_AB,SPEED_SLOW,-100); //反時計回りに旋回 while(MotorTachoCount(OUT_A)<=angle){ if (SensorUS(S1)<d_min){ //現在の距離が仮の最小値より小さい場合 d_min = SensorUS(S1); //仮の最小値を更新 tacho_min = MotorTachoCount(OUT_A); //この時のタイヤの回転数を記録 } } OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A)<=tacho_min || SensorUS(S1)<=d_min);
Wait(14); //微調整 Off(OUT_AB);Wait(500); return d_min; } sub c_up(float d) //645で最大 { RotateMotor(OUT_C,-30,d); Off(OUT_C); } sub c_down(float d) { RotateMotor(OUT_C,30,d); Off(OUT_C); }
task main() { SetSensorLowspeed(S1);
//紙コップの方向を探した後、近づいて25cm手前で停止
int d = searchDirection(30); if (d > 10){ fwdDist(d-25.0); }
turnAng(200);
c_down(640);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(12.0);
c_up(260); //紙コップをつかむ
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(110);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(11.0);
c_up(330); //ピンポン玉をはなす
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(90);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); fwdDist(17.0);
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(80);
ResetTachoCount(OUT_AB); //角度計測をリセット int e = searchDirection(40);
int msg; // 受け取った値を格納する変数 while (true) { ReceiveRemoteNumber(MAILBOX1,true,msg); if (msg == 1){ break; } else { Off(OUT_AB); } Wait(100); }
Off(OUT_AB);Wait(3000);
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(30);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); fwdDist(15.0);
ResetTachoCount(OUT_AB); //角度計測をリセット int f = searchDirection(30); if (f > 10){ fwdDist(f-25.0); }
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(156);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(8.0);
c_down(100);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(2.0);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); //微調整 Off(OUT_AB); fwdDist(12.0);
c_up(100);
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(230);
ResetTachoCount(OUT_AB); //角度計測をリセット int g = searchDirection(20); if (g > 10){ fwdDist(g-25.0); }
turnAng(190);
c_down(630);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(16.0);
c_up(270);
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(140);
c_up(340);
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(290);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(12.0);
ResetTachoCount(OUT_AB); //角度計測をリセット int h = searchDirection(20); if (h > 10){ fwdDist(h-25.0); }
ResetTachoCount(OUT_AB); //角度計測をリセット turnAng(156);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(8.0);
c_down(100);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); baDist(2.0);
OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE); Off(OUT_AB); fwdDist(12.0);
}
残念ながら私はこのプログラムをつくっておらずあまり詳しくないので詳しい説明は2017b/Member/Masa/Mission3や2017b/Member/Taka/Mission3を見てください。
このプログラムは私のプログラムとは違って超音波センサーを使っています。AをスタートしてBの延長線上の紙コップを超音波センサーを使いながら近づいて持ち上げ割りばしの中にピンポン玉を入れて、その後ロボット1から合図かあるまで待機し、センサーを使ってY地点の紙コップを検知し、近づいて上に紙コップを重ねます。そのあと残った紙コップをセンサーで検知して掴んで、ピンポン玉を割りばしの中に入れて、Y地点の紙コップに重ねて終了です。
元々はある程度近づいたら超音波センサーを使って正確に紙コップをとるようにプログラムを作っていましたが、何故かturnAngを直前に使用すると超音波センサーで紙コップを探すサブ関数がうまく動きませんでした、詳しくは角度を指定した旋回が起こらず、微動だにせずに探索が終了していました。どうやら RotateMotorExに未知の仕様があるようです。
また正確に左旋回をするために
sub turnLg(long ang) { long angle = (track/diameter)*ang; //旋回角度からタイヤの回転を計算 ResetTachoCount(OUT_AB); //角度計測をリセット OnFwdSync(OUT_AB,SPEED_SLOW,-100); // 反時計回りに旋回 until(MotorTachoCount(OUT_A)==angle); Wait(20); Off(OUT_AB); }
というサブ関数を定義して動かしてみましたが、このサブ関数の直後のプログラムが正常に動作しなくなり実装は見送りました。
今回の課題はテスト期間と重なりかつ期間の短かったので十分に満足できるものを作ることができませんでした。しかしながら同じ期間で非常に高得点を出した班もあり自分の力もまだまだだと思いました。考えているようにプログラムを作っているつもりなのに考えたように動かないことも多く、まだまだ改善の余地があると感じました。