今回はRCX二台を使っての操作だったのでプログラムも二つ存在している。 趣旨としてはまっさきに一番近い缶のところに行き、缶を掴んでそのまま直進。 中身の入った缶にまで行き、確実に一つ置けるようにしようとした。 しかし、通信機能がうまく働かず、そのまま終わってしまった。
まずはタイヤなどを動かすプログラム, ライントレースなど今までの授業の集大成です。
#define THERSHOLD 30 #define go_st OnFwd(OUT_A); OnFwd(OUT_C); #define go_ri OnFwd(OUT_A); OnRev(OUT_C); #define go_le OnFwd(OUT_C); OnRev(OUT_A); #define go_ba OnRev(OUT_A); OnRev(OUT_C); #define BLACK 38 #define WHITE 38 #define SIGNALON1 11 // アーム動作開始を知らせるメッセージ緑 #define SIGNALON2 21 // アーム動作完了を知らせるメッセージ緑 #define SIGNALON3 31 // アーム動作開始を知らせるメッセージ黒 #define SIGNALON4 41 // アーム動作完了を知らせるメッセージ黒 #define ReMessage2 32 // SIGNALON3の返事 #define RUN_SECTION2 100 //第4セクションの時間 int cross; //黒線をまたぐ回数 sub count() { if ((SENSOR_1 < THERSHOLD) && (SENSOR_3 < THERSHOLD)) {go_st;cross = cross++;} else if(SENSOR_1>THERSHOLD && SENSOR_3<THERSHOLD){ //3だけ黒線の上 OnFwd(OUT_A); Rev(OUT_C); }else if(SENSOR_1<THERSHOLD && SENSOR_3>THERSHOLD){ //1だけ黒線の上 OnFwd(OUT_C); Rev(OUT_A); }else if(SENSOR_1>THERSHOLD && SENSOR_3>THERSHOLD){ //両方黒線の外 OnFwd(OUT_AC); }} task line_1st() //90度回転:(L,R)=(B,B) 右に旋回:(L,R)=(B,W) 左に旋回:(L,R)=(W,B) 後ろに直進:(L,R=(W,W) { if ((SENSOR_1 < BLACK) && (SENSOR_3 > WHITE)) { go_le; } else if ((SENSOR_1 > WHITE) && (SENSOR_3 < BLACK)) { go_ri; } else if ((SENSOR_1 < BLACK) && (SENSOR_3 < BLACK)) { go_ri; Wait(180); //90回転させる 回転センサーをつかったほうがいいかも Off(OUT_AC); stop line_1st; //このタスクを止める } else if ((SENSOR_1 > WHITE) && (SENSOR_3 > WHITE)) { go_ba; } } task main () { SetSensor(SENSOR_1, SENSOR_LIGHT); SetSensor(SENSOR_2, SENSOR_TOUCH); SetSensor(SENSOR_3, SENSOR_LIGHT); SetPower(OUT_A,3); SetPower(OUT_C,3); ClearMessage(); ClearTimer(0); while(cross < 1){ count() }//最初の黒線を無視する while (Message() != SIGNALON1) { //真ん中が反応するまで左側の円の上を通る if ((SENSOR_1 < THERSHOLD) && (SENSOR_3 < THERSHOLD)) {go_st;cross = cross++;} if(SENSOR_1>THERSHOLD && SENSOR_3<THERSHOLD){ //3だけ黒線の上 OnFwd(OUT_A); Rev(OUT_C); }else if(SENSOR_1<THERSHOLD && SENSOR_3>THERSHOLD){ //1だけ黒線の上 OnFwd(OUT_C); Rev(OUT_A); }else if(SENSOR_1>THERSHOLD && SENSOR_3>THERSHOLD){ //両方黒線の外 OnFwd(OUT_AC); } } Off(OUT_AC); SetPower(OUT_A,3); SetPower(OUT_C,3); until (Message() = SIGNALON2); go_st; Wait(30); until ((SENSOR_1 < BLACK) || (SENSOR_3 < BLACK) || (FastTimer(0) <= RUN_SECTION2)); //センサーか時間で反応 go_le; Wait(180); go_st; until (SENSOR_2 == 1); //タッチのセンサーに缶が反応 Off(OUT_AC); PlaySound(SOUND_CLICK); SetPower(OUT_A,3); SetPower(OUT_C,3); while ( Message() != ReMessage2) { //メッセージを送り、缶を下ろさせる。 SendMessage(SIGNALON3); } if (Message() == ReMessage2) { PlaySound(SOUND_CLICK); // 送信に成功したときはピッと鳴らす } else { PlaySound(SOUND_LOW_BEEP); // 送信に失敗したときはブッと鳴らす } until (Message() = SIGNALON4); go_ba; Wait(50); Off(OUT_AC); }
次にアーム側のプログラムです。
こちら側のプログラムの主な役割はセンサーで缶に反応し、
通信を送り、アームで掴み、上げることです。
#define SIGNALON1 11 // アーム動作開始を知らせるメッセージ #define SIGNALON2 21 // アーム動作完了を知らせるメッセージ #define ReMessage1 12 // SIGNALON1の返事 #define SIGNALON3 31 // アーム動作開始を知らせるメッセージ黒 #define SIGNALON4 41 // アーム動作完了を知らせるメッセージ黒 #define ReMessage2 32 // SIGNALON3の返事 #define GREEN 51 // until (SENSOR_3 > GREEN); 普段と逆で行う int i; // for ループのための変数 int i_min=1,i_max=3; task main () { ClearMessage(); // Message()の値を0に初期化 SetSensor(SENSOR_1, SENSOR_LIGHT); SetSensor(SENSOR_3, SENSOR_ROTATION); SetPower(OUT_A,4); SetPower(OUT_B,7); for (i=i_min; i<=i_max; i++) { //3回繰り返す until (SENSOR_1 > GREEN && FastTimer()==200); PlaySound(SOUND_CLICK); SendMessage(SIGNALON1); // 缶を見つけたことを知らせる Wait(50); OnFwd(OUT_A); Wait(50); Off(OUT_A); // アームで掴む OnFwd(OUT_B); until (SENSOR_3 == -25); //回転センサーを使い一定まで上げさせる。 Off(OUT_B); // リフトであげる SendMessage(SIGNALON2); // 動作完了の合図 until (Message() == SIGNALON3); SendMessage(ReMessage2); //返事 SetPower(OUT_A,3); SetPower(OUT_B,4); OnRev(OUT_B); Wait(10); Float(OUT_B); Wait(30); //シャフトを缶まで下げる OnRev(OUT_A); Wait(50); Off(OUT_A); //アームを開く SetPower(OUT_B,7); OnRev(OUT_B); Wait(70); Float(OUT_B); //シャフトを一番したまで下げる SendMessage(SIGNALON4); } }