[[2016a/Member]] *課題 [#hd873cb3] 課題2で使用したコースに紙コップを置きそれを運び、積むロボットを作成する。 詳しくは[[こちら:http://yakushi.shinshu-u.ac.jp/robotics/?2016a%2FMission3]]。 *作戦 [#bba3bcb5] **当初の狙い [#tcaf3708] 運ぶロボット、積むロボット2つを使う。運ぶロボットはBからCへライントレースを行い、うまく置いてある紙コップを一つつかみライントレースでS、Pの真ん中まで運ぶ。それと同時に積むロボットはAからSPの真ん中までライントレースを行って待機する。すると、運ばれてくる紙コップが積むロボットのタッチセンサーに触れるのでそしたら運ぶロボットは離れて積むロボットがつかみ、右旋回を行い、置いたら元の位置に戻る。次に運ぶロボットがDへ行き、、、というのを繰り返し、6個積み終わったら最後に積むロボットが少し進めばQに置くことができる。 **当日の狙い [#p27daa8c] 運ぶロボットだけを使用して、BからCへライントレースを行い、紙コップをつかんで右折することで一つだけQに入れて3点を取りに行った。 *構造 [#c0e24a89] **運ぶロボット [#c9687b8d] 左右にモーターをつけ、それにタイヤをつけた。 ライントレース用に本体の真下に光センサーをつけた。 前方には紙コップを1つ運ぶための動かないアームをつけた。 **積むロボット [#e684ad73] 左右にモーターをつけ、モーターから少し離すようにそれぞれタイヤをつけた。 もう一つのモーターにはギアをたくさんかませたり、輪ゴムでつないだりすることで、つかむ、はなす、上下に動かす、ことを可能にしてある。 正面には紙コップが運ばれてきたのを確認するためのタッチセンサーがつけられている。 *プログラム [#iecc4cd3] **運ぶロボット(予定) [#l15b1c10] <定義> #define turn_right_SENKAI OnFwd(OUT_A); OnRev(OUT_C); //旋回 #define turn_right OnFwd(OUT_A); Off(OUT_C); //片方のみ動かす #define turn_left_SENKAI OnFwd(OUT_C); OnRev(OUT_A); //旋回 #define turn_left OnFwd(OUT_C); Off(OUT_A); //片方のみ動かす #define go_forward(t) OnFwd(OUT_AC); Wait(t); Off(OUT_AC); #define cross_straight_R turn_left; Wait(50); #define cross_straight_L turn_right; Wait(50); #define cross_right turn_right; Wait(130); #define cross_left turn_left; Wait(130); #define Black 43 //これより小さかったら黒 #define White 52 //これより大きかったら白 #define STEP 1 //一回の判断の時間 #define turn_time 400 //RからR or LからL (180°回転) #define turn_time_change 290 //RからL or LからR (90°ぐらい回転) #define closs_P 1 //closs Pを曲がる許可 #define GO_cupC 2 //Cを取りに行け #define GO_cupD 3 //Dを取りに行け #define COME_cupC 4 //Cを持って来い #define COME_cupD 5 //Dを持って来い <サブルーチン> sub trace_line_for_cross_R() //ラインの右側を交差点までトレース { ClearTimer(0); while (FastTimer(0) < 36) { if (SENSOR_2 <= Black) { //黒 turn_right_SENKAI; } else if (SENSOR_2 < Black +2) { //黒に近いグレー turn_right; } else if (SENSOR_2 > White -2) { //白に近いグレー turn_left; ClearTimer(0); } else if (SENSOR_2 >= White) { //白 turn_left_SENKAI; ClearTimer(0); } else { //グレー OnFwd(OUT_AC); ClearTimer(0); } Wait(STEP); } Off(OUT_AC); PlaySound(SOUND_FAST_UP); Wait(100); } sub trace_line_for_cross_L() //ラインの左側を交差点までトレース { ClearTimer(0); while (FastTimer(0) < 36) { if (SENSOR_2 <= Black) { //黒 turn_left_SENKAI; } else if (SENSOR_2 < Black +2) { //黒に近いグレー turn_left; } else if (SENSOR_2 > White -2) { //白に近いグレー turn_right; ClearTimer(0); } else if (SENSOR_2 >= White) { //白 turn_right_SENKAI; ClearTimer(0); } else { //グレー OnFwd(OUT_AC); ClearTimer(0); } Wait(STEP); } Off(OUT_AC); PlaySound(SOUND_FAST_UP); Wait(100); } sub trace_line_to_start_R() //ラインの右を設定時間までトレース { ClearTimer(0); while (FastTimer(0) < 50) { //要調整 if (SENSOR_2 <= Black) { //黒 turn_right_SENKAI; } else if (SENSOR_2 < Black +2) { //黒に近いグレー turn_right; } else if (SENSOR_2 > White -2) { //白に近いグレー turn_left; } else if (SENSOR_2 >= White) { //白 turn_left_SENKAI; } else { //グレー OnFwd(OUT_AC); } Wait(STEP); } } sub trace_line_to_start_L() //ラインの左側を設定時間までトレース { ClearTimer(0); while (FastTimer(0) < 50) { //要調整 if (SENSOR_2 <= Black) { //黒 turn_left_SENKAI; } else if (SENSOR_2 < Black +2) { //黒に近いグレー turn_left; } else if (SENSOR_2 > White -2) { //白に近いグレー turn_right; } else if (SENSOR_2 >= White) { //白 turn_right_SENKAI; } else { //グレー OnFwd(OUT_AC); } Wait(STEP); } } task go_to_CD() //(サブルーチンを含むのでtaskで表記) { int cupC = 0; int cupD = 0; repeat(3){ int n = 0; int m = 0; while (n < 1) { if(Message() == GO_cupD) { //C貰ったからDに行ってこい PlaySound(SOUND_UP); OnRev(OUT_AC); Wait(250); //要確認 Off(OUT_AC); turn_right; Wait(turn_time); //LからLへ trace_line_for_cross_L(); cross_left; trace_line_for_cross_L(); if(cupD == 0) { turn_right; Wait(turn_time_change); trace_line_for_cross_R(); } if(cupD == 1) { go_forward(50); //要確認 trace_line_for_cross_R(); turn_right; Wait(turn_time); //180°回転 trace_line_for_cross_R(); cross_right; trace_line_for_cross_R(); } if(cupD == 2) { while (SENSOR_2 > Black) { OnFwd(OUT_AC); } turn_right; Wait(turn_time); while (SENSOR_2 > Black) { OnFwd(OUT_AC); } cross_right; trace_line_for_cross_R(); } cupD++; while (m < 1) { if(Message() == COME_cupD) { //C置いたからD持って来い PlaySound(SOUND_UP); n++; m++; trace_line_to_start_R(); } else { Off(OUT_AC); } Wait(STEP); } } else { Off(OUT_AC); } Wait(STEP); } while (n < 2) { if(Message() == GO_cupC) { //D貰ったからCに行ってこい PlaySound(SOUND_UP); OnRev(OUT_AC); Wait(250); //要確認 Off(OUT_AC); turn_left; Wait(turn_time); //RからRへ trace_line_for_cross_R(); cross_right; trace_line_for_cross_R(); if(cupC == 0) { go_forward(50); //要確認 trace_line_for_cross_L(); turn_right; Wait(turn_time); //180°回転 trace_line_for_cross_L(); cross_left; trace_line_for_cross_L(); } if(cupC == 1) { while (SENSOR_2 > White) { turn_left; } while (SENSOR_2 > Black) { OnFwd(OUT_AC); } turn_right; Wait(turn_time); while (SENSOR_2 > Black) { OnFwd(OUT_AC); } cross_left; trace_line_for_cross_L(); } cupC++; while (m < 2) { if(Message() == COME_cupC) { //D置いたからC持って来い PlaySound(SOUND_UP); n++; m++; trace_line_to_start_L(); } else { Off(OUT_AC); } Wait(STEP); } } else { Off(OUT_AC); } Wait(STEP); } } } <本文> task main() { SetSensor(SENSOR_2, SENSOR_LIGHT); ClearMessage(); int cupC = 0; int cupD = 0; int n = 0; go_forward(50); repeat (2) { trace_line_for_cross_R(); cross_straight_R; } trace_line_for_cross_R(); cross_right; trace_line_for_cross_R(); turn_right; Wait(turn_time_change); //90°旋回(RからLへ) while(SENSOR_2 > White) { OnFwd(OUT_AC); } Off(OUT_AC); trace_line_for_cross_L(); cross_left; trace_line_to_start_L(); //要確認 SendMessage(closs_P); //用意できたからコッチに来て PlaySound(SOUND_CLICK); start go_to_CD; } **積むロボット(予定) [#l7500f33] <定義> #define turn_right_SENKAI OnFwd(OUT_A); OnRev(OUT_C); //旋回 #define turn_right OnFwd(OUT_A); Off(OUT_C); //片方のみ動かす #define turn_left_SENKAI OnFwd(OUT_C); OnRev(OUT_A); //旋回 #define turn_left OnFwd(OUT_C); Off(OUT_A); //片方のみ動かす #define go_forward(t) OnFwd(OUT_AC); Wait(t); Off(OUT_AC); #define Black 43 //これより小さかったら黒 #define White 47 //これより大きかったら白 #define STEP 1 //一回の判断の時間 #define age_sage 350 //上げたり下げたりする時間 #define closs_P 1 //closs Pを曲がる許可 #define GO_cupC 2 //Cを取りに行け #define GO_cupD 3 //Dを取りに行け #define COME_cupC 4 //Cを持って来い #define COME_cupD 5 //Dを持って来い <サブルーチン> sub for_cross() //黒まで進む { ClearTimer(0); while (FastTimer(0) < 60) { if (SENSOR_2 <= Black +2) { //黒 Off(OUT_AC); } else { OnFwd(OUT_AC); ClearTimer(0); } Wait(STEP); } Off(OUT_AC); PlaySound(SOUND_FAST_UP); Wait(100); } sub trace_line() //ライントレース { while (true) { if (SENSOR_2 <= Black) { //黒 turn_right_SENKAI; } else if (SENSOR_2 < Black +2) { //黒に近いグレー turn_right; } else if (SENSOR_2 > White -2) { //白に近いグレー turn_left; } else if (SENSOR_2 >= White) { //白 turn_left_SENKAI; } else { //グレー OnFwd(OUT_AC); } Wait(STEP); } } sub crossP() //メッセージ(closs_P)が来たら1.2秒前進 { ClearTimer(0); while (FastTimer(0) < 120 ) { if (Message() == closs_P) { PlaySound(SOUND_UP); OnFwd(OUT_AC); } else { Off(OUT_AC); ClearTimer(0); } Wait(STEP); } } sub tumutumu_C() //Cの紙コップを積む { int cupC = 0; OnFwd(OUT_B); //放す&下げる Wait(100); Off(OUT_B); while (cupC < 1) { if (SENSOR_1 == 1) { //コップ触ってたら SendMessage(GO_cupD); //Dに行ってこい(ただし交差点で待機) PlaySound(SOUND_CLICK); Wait(200); //少し待機 OnRev(OUT_B); //掴む&上げる Wait(age_sage -100); //これはやってみないと分からない。 Off(OUT_B); turn_right; //右向く Wait(200); //これも適当 Off(OUT_AC); OnFwd(OUT_B); //放す&下げる Wait(age_sage); Off(OUT_B); while (SENSOR_2 > Black) { //調整必要 turn_left; } Off(OUT_AC); SendMessage(COME_cupD); //さあ持って来い PlaySound(SOUND_CLICK); cupC++; } else { //コップ触ってなかったら Off(OUT_AC); } Wait(STEP); } OnRev(OUT_B); Wait(100); } sub tumutumu_D() //Dの紙コップを積む { int cupD = 0; OnFwd(OUT_B); //放す&下げる Wait(100); Off(OUT_B); while (cupD < 1) { if (SENSOR_1 == 1) { //コップ触ってたら SendMessage(GO_cupC); //Cに行ってこい(ただし交差点で待機) PlaySound(SOUND_CLICK); Wait(200); //少し待機 OnRev(OUT_B); //掴む&上げる Wait(age_sage -100); //これはやってみないと分からない。 Off(OUT_B); turn_right; //右向く Wait(200); //これも適当 Off(OUT_AC); OnFwd(OUT_B); //放す&下げる Wait(age_sage); Off(OUT_B); while (SENSOR_2 > Black) { //調整必要 turn_left; } Off(OUT_AC); SendMessage(COME_cupC); //さあ持って来い PlaySound(SOUND_CLICK); cupD++; } else { //コップ触ってなかったら Off(OUT_AC); } Wait(STEP); } OnRev(OUT_B); Wait(100); } <本文> task main() { SetSensor(SENSOR_1, SENSOR_TOUCH); SetSensor(SENSOR_2, SENSOR_LIGHT); ClearMessage(); int n = 0; int cupC = 0; int cupD = 0; Wait(1000); OnFwd(OUT_AC); Wait(100); for_cross(); OnRev(OUT_B); Wait(100); while (n < 1) { if (Message() == closs_P) { crossP(); n++; } else { Off(OUT_AC); } Wait(STEP); } while (n < 2) { if (SENSOR_1 == 0) { trace_line(); } else { Off(OUT_AC); n++; } Wait(STEP); } repeat (3) { tumutumu_C(); tumutumu_D(); } turn_right; Wait(200); OnFwd(OUT_AC); Wait(400); Off(OUT_AC); } **運ぶロボット(本番) [#ud2182b2] <定義> #define turn_right_SENKAI OnFwd(OUT_A); OnRev(OUT_C); //旋回 #define turn_right OnFwd(OUT_A); Off(OUT_C); //片方のみ動かす #define turn_left_SENKAI OnFwd(OUT_C); OnRev(OUT_A); //旋回 #define turn_left OnFwd(OUT_C); Off(OUT_A); //片方のみ動かす #define go_forward(t) OnFwd(OUT_AC); Wait(t); Off(OUT_AC); #define cross_straight_R turn_left; Wait(50); #define cross_straight_L turn_right; Wait(50); #define cross_right turn_right; Wait(160); #define cross_left turn_left; Wait(160); #define Black 43 //これより小さかったら黒 #define White 52 //これより大きかったら白 #define STEP 1 //一回の判断の時間 #define turn_time 400 //RからR or LからL (180°回転) #define turn_time_change 300 //RからL or LからR (90°ぐらい回転) #define closs_P 1 //closs Pを曲がる許可 #define GO_cupC 2 //Cを取りに行け #define GO_cupD 3 //Dを取りに行け #define COME_cupC 4 //Cを持って来い #define COME_cupD 5 //Dを持って来い <サブルーチン>