![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#author("2020-02-13T23:16:39+09:00","atsu","atsu") #author("2020-02-13T23:17:51+09:00","atsu","atsu") 目次 #contents * 課題３ [#p799c328] * 課題３ [#s8c83ad2] [[2019b/Mission3]]を参照。 * ロボットの説明 [#c4392f25] * ロボットの説明 [#a07cec52] 前回の反省を生かして、ボールを取る機構と置く機構、センサー類が全て同じ方向を向く構造にした。 制御ユニットは二つ使用した。 #ref(2019b/Member/atsu/Mission3/3_robot_all.jpg,100%,ロボットの全体像) ** 制御ユニット１（マスター） [#w444029b] ** 制御ユニット１（マスター） [#x6885cac] *** ボールをつかむ機構 [#r9ef60de] *** ボールをつかむ機構 [#xab1d134] 空き缶はとても軽いので、動かないようにするには鉛直方向のみに力をかけるしかないと考えた。上からしか力をかけないことと、ボールの位置に誤差があったとしても確実にボールを掴めることを実現するために、輪ゴムを使用した機構にした。 具体的には、二本の輪ゴムをボールの直径よりも一回り小さく張った。このような構造にすることで、ボールに上から押しつけたときは、ボールがゴムを押しのけて上に出てくるが、ボールの自重ではゴムを押しのけることが出来ずに上にとどまる。この構造だと、ボールの中心が二つのゴムの間にある限り掴むことが出来る。 #ref(2019b/Member/atsu/Mission3/3_hand_c.jpg,100%,掴む機構) *** ボールを置く機構 [#xf91f670] *** ボールを置く機構 [#h0b66651] ボールを掴む機構と置く機構を同じに方向に付けたかったので、掴む機構を利用することにした。ここでも鉛直方向にしか力をかけられないので、ボールを上から落とせるような構造を考えた。 具体的には、ボールを掴む機構の上にボールをセットした上で、上からボールを押すような構造にした。ボールをセットするときのためにゴムを張ったものと９０度ずらしてアームを取り付けた。 &ref(2019b/Member/atsu/Mission3/3_hand_p.jpg,100%,置く機構); &ref(2019b/Member/atsu/Mission3/3_hand.jpg,100%,ボールをセットするためのパーツ); ボールを押すアームに超音波センサーを取り付けることで、ボールを押すときにはボールを押すことの出来る面積を増やし、使わないときには缶の位置を監視できるようにした。 *** 超音波センサー [#sd4afb9d] *** 超音波センサー [#a625ca20] この超音波センサーの精度では距離をいつも正確に測るのは難しいので、缶がそこに有るか無いかを判断するのに使用した。 具体的には、缶との距離を上から見下ろす形で計測するようにした。アームに付けたのは、ボールを置く動作の際にも使えるようにしたかったである。 具体的には、缶との距離を上から見下ろす形で計測するようにした。アームに付けたのは、ボールを置く動作の際にも使えるようにしたかったからである。 #ref(2019b/Member/atsu/Mission3/3_sensor_us.jpg,100%,超音波センサー) ** 制御ユニット２（スレイブ） [#vc2c80fb] ** 制御ユニット２（スレイブ） [#n74839c6] *** 光センサー [#u9207b53] *** 光センサー [#a86895cc] 今回は光センサーを二つ使用した。二つ使用する方が様々なことが出来ると思ったである。首振りを少なくするためにセンサーの間隔を狭くした。 今回は光センサーを二つ使用した。二つ使用する方が様々なことが出来ると思ったからである。首振りを少なくするためにセンサーの間隔を狭くした。 #ref(2019b/Member/atsu/Mission3/3_sensor_l.jpg,100%,光センサー) * プログラムの説明 [#h4e9d574] * プログラムの説明 [#s52b97fd] **今回自分が作ったプログラム [#afd2135b] **今回自分が作ったプログラム [#pc082d6a] 今回自分が作ったプログラムは、ボールを３つ掴むところまでしか出来なかったので本番では違うものを動かしたが、工夫点がいくつかあるので書きたいと思う。本番で動かしたプログラムについては次の章で書く。 今回プログラムを書いていて気付いたが、"MotorTachoCount()" と "MotorRotationCount()" は別物である。授業で配布されたプリントには、"MotorTachoCount()" がモーターの回転角を計測する関数であると書かれているが、実際には "MotorTachoCount()" はモーターの回転数を計測する関数であり、回転角を計測するには "MotorRotationCount()" を使用する必要がある。ライントレースの関数の中で交差点の判断に回転角を使用していた関係で、うまく交差点で止まれていなかった。本番前日までこれに気付けなかったのは痛かった。 *** スレイブ側（ライントレースを行う方） [#o4a6e24f] *** スレイブ側（ライントレースを行う方） [#o95f0e9b] #define Speed 40 //ロボットのスピード //光センサー #define SeL_l S1 //左光センサー S1 #define SeL_r S2 //右光センサー S2 //モーター #define M_r OUT_C //モーター右 C #define M_l OUT_B //モーター左 B #define M_w OUT_BC //両方のモーター //メッセージ #define stop_l 11 //ストップ #define finish 10 //作業終了（次の動作に移る） float B, W; //白と黒の値 const float cyokkei = 5.45; //タイヤ直径 const float haba = 11.05; //タイヤ間の距離 const float pi = 3.14; //π const float P = 40; const float I = 40; const float D = 90; //int P, I, D; inline float min(float x, float y) { return x < y ? x : y; } inline float max(float x, float y) { return x > y ? x : y; } /*--白と黒の値とPIDの値をを返す--*/ float SetBAndW() { //W and B SetSensorLight(SeL_l), SetSensorLight(SeL_r), B = 100, W = 0; float ave; do { ave = (Sensor(SeL_l) + Sensor(SeL_r)) / 2; B = min(ave, B), W = max(ave, W); OnFwd(M_w, Speed); } while (abs(ave - B) < 7); Off(M_w); B = B + 5, W = W - 3; //PID //P = MotorRegPValue(M_w); //I = MotorRegIValue(M_w); //D = MotorRegDValue(M_w); } /*--ライントレース(いくつ目の交差点で停止したいか)--*/ void line_follow(int WantStop) { SetSensorLight(SeL_l), SetSensorLight(SeL_r); int intersection = 0; //通過した交差点の数 float Sl_l, Sl_r; //光センサーの値 int msg = 0; //メッセージ float m, x, y; ResetRotationCount(M_w); //角度リセット x = (/*(Speed * (1))*/ 1 / (W - (B + 10))); //モーター出力の傾き y = pi * cyokkei / 360.0; //走行距離の計算式 while ((intersection < WantStop) && (msg != stop_l)) { Sl_l = Sensor(SeL_l), Sl_r = Sensor(SeL_r); //センサーの値を入れる m = max(Sl_l, Sl_r); OnFwd(M_l, Speed * (1 + (x * (Sl_l - m)))); //右モーター OnFwd(M_r, Speed * (1 + (x * (Sl_r - m)))); //左モーター Wait(10); if ((Sl_l <= B) && (Sl_r <= B)) //黒を通過するとき { if (((y * MotorRotationCount(M_l)) >= 1.5) || (y * MotorRotationCount(M_r)) >= 1.5) / /1.5cm以上黒を通過したとき（交差点を通過したとき） { intersection = intersection + 1; //交差点の数をカウント ResetRotationCount(M_w); //角度リセット } } else //白いとき { ResetRotationCount(M_w); //角度リセット } ReceiveRemoteNumber(MAILBOX1, true, msg); //メッセージを受信 } Off(M_w); //停止 SendResponseNumber(MAILBOX1, finish); //完了メッセージ } /*--直進（距離指定(cm)）--*/ void go_cm(float d) { ResetRotationCount(M_w); //角度リセット long angle = d / (cyokkei * pi) * 360.0; RotateMotorExPID(M_w, Speed, angle, 0, true, true, P, I, D); } /*--時計回り旋回--*/ void turn_ang(long ang) { ResetRotationCount(M_w); //角度リセット long angle = (haba / cyokkei) * ang; RotateMotorExPID(M_w, Speed, angle, 100 * sign(-ang), true, true, P, I, D); } /*--作業スタート--*/ void Start() { int msg = 0; //メッセージ do { ReceiveRemoteNumber(MAILBOX1, true, msg); //メッセージを受信 } while (msg != finish); Wait(600); } /*--缶を退ける--*/ void remove_can() { go_cm(30); Wait(700); go_cm(-29); Wait(500); SendResponseNumber(MAILBOX1, finish); //完了メッセージ } /*---------メインプログラム-----------*/ task main() { Start(); PlaySound(SOUND_CLICK); Wait(700); //ライントレース開始 SetBAndW(); PlaySound(SOUND_CLICK); //1つめを探しに行く line_follow(1); Start(); remove_can(); //2つめを探しに行く line_follow(4); //Wait(300); Start(); remove_can(); //3つめを探しに行く turn_ang(10); line_follow(1); Start(); remove_can(); //空き缶の上まで移動 PlaySound(SOUND_CLICK); line_follow(1); //G' turn_ang(-90); line_follow(1); //H' turn_ang(-90); line_follow(1); //H turn_ang(90); line_follow(1); //G turn_ang(90); //球を置く line_follow(1); //F Start(); go_cm(-3); turn_ang(180); line_follow(1); //G turn_ang(90); line_follow(1); //D turn_ang(90); //球を置く line_follow(1); //E Start(); go_cm(-3); turn_ang(180); //球を置く line_follow(2); //C Start(); go_cm(-3); } ''しきい値を決める関数'' 前回は、電池の状態によるセンサーのしきい値の変化に苦労したので、スタート時に値を自動で決めてくれる関数を作った。 具体的には、ロボットがスタート地点の黒い枠から外に出る間にセンサーの最大値と最小値を取得し、その値を基にしきい値を定める関数を作った。 工夫した点は、いつこの関数を止めるかを、電池の状態やロボットを置いた位置に依存しないようにしたところだ。ロボットを置くのは白いところであると考える。この関数を走らせると、最大値はすぐに決まってしまうが、黒い枠を超えるまでは最小値はセンサーの値とほぼ同じである。しかし、枠を超えると（本当の最小値が決まると）最小値とセンサーの値は離れていく。これを利用して関数を止めている。 #define Speed 40 //ロボットのスピード //光センサー #define SeL_l S1 //左光センサー S1 #define SeL_r S2 //右光センサー S2 //モーター #define M_r OUT_C //モーター右 C #define M_l OUT_B //モーター左 B #define M_w OUT_BC //両方のモーター float B, W; //白と黒の値 /*--白と黒の値とPIDの値をを返す--*/ float SetBAndW() { //W and B SetSensorLight(SeL_l), SetSensorLight(SeL_r), B = 100, W = 0; float ave; do { ave = (Sensor(SeL_l) + Sensor(SeL_r)) / 2; B = min(ave, B), W = max(ave, W); OnFwd(M_w, Speed); } while (abs(ave - B) < 7); Off(M_w); B = B + 5, W = W - 3; //PID //P = MotorRegPValue(M_w); //I = MotorRegIValue(M_w); //D = MotorRegDValue(M_w); } ''ライントレース'' 今回のロボットは光センサーが二つ付いているので、これを活かせるようにした。また、交差点の判断にはモーターの出力に依存しないように時間ではなく距離を使うことにした。 左右に首を振ると超音波センサーが反応しない事があったので、なるべく首を振らせないようにするために比例制御のようなものを使用した。また、左右の光センサーの値の差によってモーターの出力を決めるようにした。これによって場合分けなしでなめらかなライントレースを行ってくれるようになった。 無視する交差点を指定する時も、時間指定では電池の状態に依存してしまうので、いくつ目の交差点で止まるかを指定できるようにした。 #define Speed 40 //ロボットのスピード //光センサー #define SeL_l S1 //左光センサー S1 #define SeL_r S2 //右光センサー S2 //モーター #define M_r OUT_C //モーター右 C #define M_l OUT_B //モーター左 B #define M_w OUT_BC //両方のモーター //メッセージ #define stop_l 11 //ストップ #define finish 10 //作業終了（次の動作に移る） float B, W; //白と黒の値 /*--ライントレース(いくつ目の交差点で停止したいか)--*/ void line_follow(int WantStop) { SetSensorLight(SeL_l), SetSensorLight(SeL_r); int intersection = 0; //通過した交差点の数 float Sl_l, Sl_r; //光センサーの値 int msg = 0; //メッセージ float m, x, y; ResetRotationCount(M_w); //角度リセット x = (/*(Speed * (1))*/ 1 / (W - (B + 10))); //モーター出力の傾き y = pi * cyokkei / 360.0; //走行距離の計算式 while ((intersection < WantStop) && (msg != stop_l)) { Sl_l = Sensor(SeL_l), Sl_r = Sensor(SeL_r); //センサーの値を入れる m = max(Sl_l, Sl_r); OnFwd(M_l, Speed * (1 + (x * (Sl_l - m)))); //右モーター OnFwd(M_r, Speed * (1 + (x * (Sl_r - m)))); //左モーター Wait(10); if ((Sl_l <= B) && (Sl_r <= B)) //黒を通過するとき { if (((y * MotorRotationCount(M_l)) >= 1.5) || (y * MotorRotationCount(M_r)) >= 1.5) / /1.5cm以上黒を通過したとき（交差点を通過したとき） { intersection = intersection + 1; //交差点の数をカウント ResetRotationCount(M_w); //角度リセット } } else //白いとき { ResetRotationCount(M_w); //角度リセット } ReceiveRemoteNumber(MAILBOX1, true, msg); //メッセージを受信 } Off(M_w); //停止 SendResponseNumber(MAILBOX1, finish); //完了メッセージ } ''通信時に使用する関数'' 具体的には、片方の作業が終わるまで待機する関数を作った。多くの関数には作業終了時に「完了」メッセージを送るプログラムを組み込んであるので、このメッセージを受け取ったら作業を開始するようにした。 //メッセージ #define stop_l 11 //ストップ #define finish 10 //作業終了（次の動作に移る） /*--作業スタート--*/ void Start() { int msg = 0; //メッセージ do { ReceiveRemoteNumber(MAILBOX1, true, msg); //メッセージを受信 } while (msg != finish); Wait(600); } *** マスター側（ボールを操作する方） [#zddee735] *** マスター側（ボールを操作する方） [#g7038bbd] #define Speed 20 //アームのスピード //超音波センサー #define Sus S4 //S4 //モーター #define M_c OUT_B //キャッチモーター B #define M_p OUT_C //プットモーター C #define M_w OUT_BC //両方のモーター BC //メッセージ #define CONN 1 #define stop_l 11 //ストップ #define finish 10 //作業終了（次の動作に移る） float d; //超音波センサーと地面の距離 //const float P = 40; //const float I = 40; //const float D = 90; /*--初期化--*/ void Initialize() { //アームの位置の初期化 OnFwd(M_c, Speed); OnFwd(M_p, 2 * Speed); Wait(2000); Off(M_w); Wait(500); ResetRotationCount(M_w); //角度リセット RotateMotor(M_p, -Speed, 40); RotateMotor(M_c, -(2 * Speed), 30); Wait(800); //床との距離を計測 SetSensorLowspeed(Sus); long sum = 0, int i = 0; while (i <= 50) { sum = sum + SensorUS(Sus); i = i + 1; } d = sum / i; SendRemoteNumber(CONN, MAILBOX1, finish); //完了メッセージ } /*--ボールをキャッチ--*/ void catch_ball() { ResetRotationCount(M_w); //角度リセット //ResetTachoCount(M_w); OnFwd(M_c, Speed); Wait(800); OnFwd(M_p, 45); Wait(1200); OnFwd(M_c, -Speed); Wait(1700); RotateMotor(M_c, Speed, 150); OnFwd(M_p, -35); Wait(700); RotateMotor(M_c, -Speed, 30); /* ResetRotationCount(M_c); ResetRotationCount(M_p); RotateMotor(M_c, Speed, 20); OnFwd(M_p, 2 * Speed); Wait(700); OnFwd(M_c, -Speed); Wait(1700); //つかむ OnFwd(M_c, Speed); Wait(1700); long ang_p = MotorRotationCount(M_p); RotateMotor(M_p, -Speed, abs(ang_p) - 5); long ang_c = MotorRotationCount(M_c); RotateMotor(M_c, -(2 * Speed), abs(ang_c) + 10); */ SendRemoteNumber(CONN, MAILBOX1, finish); //完了メッセージ } /*--ボールを置く--*/ void put_ball() { ResetRotationCount(M_w); //角度リセット RotateMotor(M_c, Speed, 30); RotateMotor(M_p, Speed, 50); Wait(1500); RotateMotor(M_p, Speed, -10); Wait(1500); RotateMotor(M_c, Speed, -100); //空き缶の上にゴム枠をセット Wait(600); RotateMotor(M_p, Speed, -30); //突き後半 //RotateMotor(M_p,Speed + 10,45); //一回腕を上げる //RotateMotor(M_p,Speed/2,-30); //２回目の突き Wait(400); RotateMotor(M_c, Speed * 2, 30); //センサを下ろしたままゴム枠を上げる RotateMotor(M_p, Speed * 2, 60); RotateMotor(M_c, Speed, 50); SendRemoteNumber(CONN, MAILBOX1, finish); //完了メッセージ } /*--ボールを探して止まる--*/ void search_ball() { SetSensorLowspeed(Sus); while (true) { if (SensorUS(Sus) < (d * (3 / 4))) //超音波センサーの値が床との距離より明らかに短い場合 { SendRemoteNumber(CONN, MAILBOX1, stop_l); //停止命令 break; } } Wait(500); } /*--作業スタート--*/ void Start() { int msg; //メッセージ do { ReceiveRemoteNumber(MAILBOX1, true, msg); //メッセージを受信 } while (msg != finish); Wait(600); } /*---------メインプログラム-----------*/ task main() { //Initialize(); //catch_ball(); Initialize(); //1つめを探しに行く search_ball(); PlaySound(SOUND_CLICK); Start(); catch_ball(); //2つめを探しに行く Start(); search_ball(); PlaySound(SOUND_CLICK); Start(); catch_ball(); //3つめを探しに行く Start(); search_ball(); PlaySound(SOUND_CLICK); Start(); catch_ball(); Start(); //空き缶の上まで移動 Start(); //G' Start(); //H' Start(); //H Start(); //G //球を置く search_ball(); //F PlaySound(SOUND_CLICK); Start(); put_ball(); Start(); //G Start(); //D //球を置く search_ball(); //E PlaySound(SOUND_CLICK); Start(); put_ball(); Wait(1000); //球を置く search_ball(); //C PlaySound(SOUND_CLICK); Start(); put_ball(); } ''アームの位置の初期化'' スタート時にアームをセットし忘れても大丈夫なようにした。 このとき同時に超音波センサーの値を測っておくようにした。超音波センサーは気まぐれなので、基準となる値（床とセンサーの距離）を求めるためだ。一応50回測って平均を出すようにしてある。 #define Speed 20 //アームのスピード //超音波センサー #define Sus S4 //S4 //モーター #define M_c OUT_B //キャッチモーター B #define M_p OUT_C //プットモーター C #define M_w OUT_BC //両方のモーター BC //メッセージ #define CONN 1 #define stop_l 11 //ストップ #define finish 10 //作業終了（次の動作に移る） float d; //超音波センサーと地面の距離 /*--初期化--*/ void Initialize() { //アームの位置の初期化 OnFwd(M_c, Speed); OnFwd(M_p, 2 * Speed); Wait(2000); Off(M_w); Wait(500); ResetRotationCount(M_w); //角度リセット RotateMotor(M_p, -Speed, 40); RotateMotor(M_c, -(2 * Speed), 30); Wait(800); //床との距離を計測 SetSensorLowspeed(Sus); long sum = 0, int i = 0; while (i <= 50) { sum = sum + SensorUS(Sus); i = i + 1; } d = sum / i; SendRemoteNumber(CONN, MAILBOX1, finish); //完了メッセージ } ''ボールを探す関数'' 超音波センサーの気まぐれに惑わされないように、センサーの値が初期化の関数で求めた床との距離の 3/4 以下になったら缶があると判断するようにしてある。首を振らないようにしたライントレースの効果もあってほぼ確実に缶を発見出来るようになった。 //メッセージ #define CONN 1 #define stop_l 11 //ストップ #define finish 10 //作業終了（次の動作に移る） float d; //超音波センサーと地面の距離 /*--ボールを探して止まる--*/ void search_ball() { SetSensorLowspeed(Sus); while (true) { if (SensorUS(Sus) < (d * (3 / 4))) //超音波センサーの値が床との距離より明らかに短い場合 { SendRemoteNumber(CONN, MAILBOX1, stop_l); //停止命令 break; } } Wait(500); } ** 実際に本番で動かしたプログラム [#g05d4a20] ** 実際に本番で動かしたプログラム [#d54f2665] 自分で書いたプログラムは３つのボールを集めるところまでしか出来なかったので、本番ではなんとか１つはボールを置けそうなプログラムを使用した。 *** スレイブ側（ライントレースを行う方） [#ne9549a5] *** スレイブ側（ライントレースを行う方） [#ubdfda06] Bluetooth接続がうまくいっているかを判定するようにしてある。 #define t 52 #define sp 40 #define go 1 #define st 2 #define con 4 #define tt 700 #define bt 1500 int y; void gf (long x,long y){ OnFwd(OUT_B,x);OnFwd(OUT_C,y); } void s_con(){//接続 SendResponseNumber(MAILBOX1,con);PlayTone(700,200); } void line_trace(long t_min) {//止まる命令が来るまでライントレース SetSensorLight(S1); SetSensorLight(S2); long t_start=CurrentTick();PlayTone(700,200); while(y!=st){ ReceiveRemoteNumber(MAILBOX1,true,y); if(SENSOR_1<t-7){ if(SENSOR_2<t-7){ if((CurrentTick()-t_start)<t_min){gf(sp,sp);} else{break;} } else if(SENSOR_2<t+7){ gf(0,sp); } else { gf(-sp,sp); } }//if1 else if(SENSOR_1<t+7){ if(SENSOR_2<t-7){ gf(sp,0); } else if(SENSOR_2<t+7){ gf(sp,sp); } else { gf(0,sp); } }//if2 else{ if(SENSOR_2<t-7){ gf(sp,-sp); } else if(SENSOR_2<t+7){ gf(sp,sp); } else { gf(sp,sp); } }//if3 }//while gf(0,0); PlayTone(1400,200); }//trace void move_can(){ OnFwdSync(OUT_BC,sp,0);Wait(3500); OnFwdSync(OUT_BC,-sp,0);Wait(2900); } void waiting(){ while(y!=go){ReceiveRemoteNumber(MAILBOX1,true,y);} } void turn_l(){ gf(sp,sp);Wait(300);gf(-sp,sp);Wait(tt); } void turn_r(){ gf(sp,sp);Wait(300);gf(sp,-sp);Wait(tt); } task main(){ s_con();//返事 waiting();//go待ち line_trace(10000);//缶1までトレース waiting();//ボール１catch待ち move_can();//缶１どかす waiting();//go待ち line_trace(20000);//缶２までトレース waiting();//ボール２キャッチ待ち move_can();//缶２どかす waiting();//go待ち gf(sp,sp);Wait(400);turn_r();line_trace(8000);//缶3までトレース waiting();//ボール３キャッチ待ち move_can();//缶３どかす waiting();//go待ち gf(sp,sp);Wait(400);turn_r();line_trace(3000);//缶３どかしたあとfg turn_l();line_trace(1500);//gh turn_l();line_trace(1000);//hh' turn_r();line_trace(1000);//h'g' gf(sp,sp);Wait(300);turn_r();line_trace(5000);//g'f' waiting();//go待ち OnFwdSync(OUT_BC,-sp,0);Wait(1000);gf(-sp,sp);Wait(tt);;line_trace(1000);//g'd' turn_r();line_trace(1000);//d'e' waiting();//go待ち OnFwdSync(OUT_BC,-sp,0);Wait(bt);//e'd' gf(sp,sp);Wait(300);gf(sp,-sp);Wait(tt*2);line_trace(10000);//d'c' } *** マスター側（ボールを操作する方） [#e7caf798] *** マスター側（ボールを操作する方） [#cbba882b] こちらも超音波センサーであらかじめ床との距離を測っておいて、その値を基に缶の有無を判断している。 #define go 1 #define st 2 #define con 4 #define spd 30 void m_con(){//接続 int x; while(x!=4){ ReceiveRemoteNumber(MAILBOX1,false,x); } PlayTone(700,200); } void reset(){//初期位置 ResetTachoCount(OUT_BC); OnFwd(OUT_B,20);Wait(800); OnFwd(OUT_C,45);Wait(1200); RotateMotor(OUT_B,-20,10); OnFwd(OUT_C,-20);Wait(1700); OnFwd(OUT_B,-20);Wait(400); Off(OUT_BC); } void observe(){ int c=0; long sum=0; float a; SendRemoteNumber(1,MAILBOX1,go); SetSensorLowspeed(S4); while(c!=10){ sum=sum+SensorUS(S4); c=c+1; } a=sum/10; while(SensorUS(S4)>0.5*a){ }//while SendRemoteNumber(1,MAILBOX1,st); }//void void catch(){ Wait(100); ResetTachoCount(OUT_BC); OnFwd(OUT_B,20);Wait(800); OnFwd(OUT_C,45);Wait(1200); OnFwd(OUT_B,-20);Wait(1700); RotateMotor(OUT_B,20,150); OnFwd(OUT_C,-35);Wait(700); RotateMotor(OUT_B,-20,30); SendRemoteNumber(1,MAILBOX1,go); } void put_ball() {/* ResetTachoCount(OUT_BC); RotateMotor(OUT_B,spd,30); RotateMotor(OUT_C,spd,50); Wait(1500); RotateMotor(OUT_C,spd,-10); Wait(1500); RotateMotor(OUT_B,spd,-100); //空き缶の上にゴム枠をセット Wait(600); RotateMotor(OUT_C,spd,-30); //突き後半 Wait(400); RotateMotor(OUT_B,spd * 2,30); //センサを下ろしたままゴム枠を上げる RotateMotor(OUT_C,spd * 2,60); RotateMotor(OUT_B,spd,50); */ OnFwd(OUT_B,spd); Wait(1000); OnFwd(OUT_C,2 * spd); RotateMotor(OUT_B,4 * spd,-5); Wait(8000); Off(OUT_BC); RotateMotor(OUT_B,spd,-120); //空き缶の上にゴム枠をセット Wait(600); RotateMotor(OUT_C,spd,-65); //突き Wait(400); RotateMotor(OUT_B,spd + 10,50); //センサを下ろしたままゴム枠を上げる Wait(500); OnFwd(OUT_C,2 * spd); Wait(1000); RotateMotor(OUT_B,spd,70); RotateMotor(OUT_C,spd,-40); RotateMotor(OUT_B,spd,-30); SendRemoteNumber(1,MAILBOX1,go); } task main(){ m_con(); reset(); observe();//ボール１を観測 catch();//ボール１つかむ Wait(7000);//缶１どかされるまで待つ observe();//ボール2を観測 catch();//ボール２をつかむ Wait(7000);//缶2どかされるまで待つ observe();//ボール３を観測 catch();//ボール３をつかむ Wait(7000);//缶３どかされるまで待つ SendRemoteNumber(1,MAILBOX1,go); Wait(10000); observe(); put_ball();//1 observe(); put_ball();//2 observe(); put_ball();//3 } * 結果 [#aab32964] * 結果 [#a80521b6] 出来はあまりよくなかった。ボールを掴んでから置きに行くことが出来なかった。 * 反省点 [#ddd845fd] * 反省点 [#k38fd711] "MotorTachoCount()" と "MotorRotationCount()" の違いにはもっと早く気付くべきだった。前回の課題の時に使った「空き缶を探すプログラム」のときから思い通りの動きをしてくれないことには気付いていたが、使用した関数が原因だとは考えもしなかった。前回の経験が活きていなかった。 ボールを置く動作をもっと簡単に行えるような構造にしておけばよかったかもしれない。 欲張って３つ一気に運ぼうとしたのが行けなかったのかもしれない。 * 最後に [#w189d5c7] * 最後に [#a579863b] 時間はだいぶかかったが、面白い課題だと思った。
