2006a/MemberOnly/進行状況A

光源を追いかけるロボット

工夫した点

苦労した点

〜感想〜

プログラムの流れ

  1. 最初は旋回してライトを探しその方向に進む。ライトの強さからライトが当たっているときの光の強さの平均を出し、その値からしきい値を計算する。
  2. 左右のセンサの差が一定以内なら直進、ある値よりも差が大きかったら「SetPower」で出力を調整して曲がる。
  3. 左右のライトがどちらもしきい値以下だったらライトを見失ったとして、直前のセンサ入力から左右に旋回しライトを探す。
  4. 上の2、3を繰り返して走行する。

プログラム

ライトを見失ったとき演奏

// programing :Taro horikawa

#define S10 SENSOR_1  //センサ1をS10と定義
#define S20 SENSOR_2  //センサ2をS20と定義
#define S30 SENSOR_3  //センサ3をS30と定義
#define F_L OnFwd(OUT_A)  //左輪前進をF_Lと定義
#define F_R OnFwd(OUT_C) //左輪後退をF_Rと定義
#define R_L OnRev(OUT_A) //右輪前進をR_Lと定義
#define R_R OnRev(OUT_C) //右輪後退をR_Rと定義

#define C  523
#define Ces 554
#define D  587
#define Des 622
#define E  659
#define F  698
#define Fes 740
#define G 784
#define Ges 831
#define A 880
#define Aes 932
#define H 988
#define C8  1047
#define Ces8 1109
#define D8 1175
#define Des8 1245
#define E8 1319
#define F8 1397
#define  Fes8 1480
#define  G8 1568
#define  Ges8 1661
#define  A8 1760
#define  Aes8 1865 

task music1()         //演奏タスク
{
   while (true)   //演奏
   {
PlayTone(C,58); Wait(60);
PlayTone(F,38); Wait(40);
PlayTone(C,8); Wait(10);
PlayTone(F,18); Wait(20);
PlayTone(Ges,58); Wait(60);
PlayTone(F,58); Wait(60);
PlayTone(Ges,37); Wait(39);
PlayTone(F,8); Wait(10);
PlayTone(Ges,18); Wait(20);
PlayTone(C8,58); Wait(60);
PlayTone(Ges,58); Wait(60);  

PlayTone(C8,37); Wait(39);
PlayTone(Ges,8); Wait(10);
PlayTone(C8,18); Wait(20);
PlayTone(Des8,58); Wait(60);
PlayTone(Des,58); Wait(60);  
PlayTone(Ges,38); Wait(40);
PlayTone(Des,8); Wait(10);
PlayTone(Ges,32); Wait(34);
PlayTone(C8,120); Wait(120); 
Wait(200);
       
       
   }
}
         
task main(){                              //メインループ

             SetPower(OUT_A,10);  //左右のモータ出力を最大にしておく
             SetPower(OUT_C,10);  //
SetSensor(S10,SENSOR_LIGHT);  //センサ1を光センサと定義
SetSensor(S30,SENSOR_LIGHT);  //センサ3を光センサと定義
SetSensor(S20,SENSOR_TOUCH);//センサ2をタッチセンサと定義
int Lmax=0,Rmax=0,Cmax=0,Ltime=0,Rtime=0,Ctime=0;

//  Lmax:  左センサ最高光度  Rmax:  右センサ最高光度  Cmax:  Lmax,Rmaxの平均(光源が真正面に来たときの予測値)
//  Ltime:  左センサが最大になったときの時間  Rtime:  右センサが ” ”
//  Ctime:  Ltime,Rtimeの平均(光源が真正面に来たときの予測時間)

Wait(70);
     ClearTimer(0);  //タイマー0をリセット
     while(FastTimer(0)<=430){  //はじめに1周旋回し、ライトを探す
      
         if(Lmax<=S10){ //左センサが最大になった時間とその値を保存
               Lmax=S10;
               Ltime=FastTimer(0);  
                            }
                            
         if(Rmax<=S30){ //上に同じ
               Rmax=S30;
               Rtime=FastTimer(0);
                            }
          OnFwd(OUT_A); //右旋回
          OnRev(OUT_C); //
      
                                          }
          Off(OUT_A+OUT_C);
          Wait(30);
      Ctime=430 - (Rtime+Ltime)/2;  //ライトが真正面にきたと思われる時間を計算
          OnFwd(OUT_C);   ////
          OnRev(OUT_A);  //ライトの方向を向く
          Wait(Ctime);       //// 
          Off(OUT_A+OUT_C);
          Cmax=(Lmax+Rmax)/2;   //左右のセンサの最大値の平均
          
     while(true){

if(S10>Cmax-12 || S30>Cmax-12){        //左右のセンサのどちらか一方でもしきい値以上だったとき
    
        if((S10<=S30+5) && (S10>=S30-5)){   //左右のセンサの差が許容範囲内だったら直進
             SetPower(OUT_A,10);
             SetPower(OUT_C,10);
                                                    }
        if(S10<=S30-6){                               //左センサが右センサよりも小さかったら右モータの出力を下げる
         Off(OUT_C);
            Wait(5);
             SetPower(OUT_A,10);
             SetPower(OUT_C,3);
        Lmax=S10;
        Rmax=S30;
                          }
        if(S30<=S10-6){                               //上の処理の逆
         Off(OUT_A);
            Wait(5);
             SetPower(OUT_A,3);
             SetPower(OUT_C,10);
        Lmax=S10;
        Rmax=S30;
                          }
                                 
                                            
            F_R;
            F_L;
            
                                            }
            
         if(S10<=Cmax-12 && S30<=Cmax-12){   //ライトを見失ったとき
             SetPower(OUT_A,10);
             SetPower(OUT_C,10);
              start music1;
until((S10<=S30+6) && (S10>=S30-6) && (S10>Cmax-12 || S30>Cmax-12)){  //ライトが正面に来るまで以下の処理
                         if(Lmax>=Rmax){     //直線のセンサ入力が左センサの方が大きかったら左旋回
                         F_R;R_L;
                                                   }
                         if(Lmax<Rmax){       //上の処理の逆
                         F_L;R_R;
                                                 }
                                                                                                             }
                                      
                                                             }
           stop music1;
     
                     }
 
               }

近づきすぎると停止

// programing :Taro horikawa

#define S10 SENSOR_1  //センサ1をS10と定義
#define S20 SENSOR_2  //センサ2をS20と定義
#define S30 SENSOR_3  //センサ3をS30と定義
#define F_L OnFwd(OUT_A)  //左輪前進をF_Lと定義
#define F_R OnFwd(OUT_C) //左輪後退をF_Rと定義
#define R_L OnRev(OUT_A) //右輪前進をR_Lと定義
#define R_R OnRev(OUT_C) //右輪後退をR_Rと定義

#define C  523
#define Ces 554
#define D  587
#define Des 622
#define E  659
#define F  698
#define Fes 740
#define G 784
#define Ges 831
#define A 880
#define Aes 932
#define H 988
#define C8  1047
#define Ces8 1109
#define D8 1175
#define Des8 1245
#define E8 1319
#define F8 1397
#define  Fes8 1480
#define  G8 1568
#define  Ges8 1661
#define  A8 1760
#define  Aes8 1865 

task music1()      //演奏タスク
{
   while (true)         //演奏
   {
PlayTone(C,58); Wait(60);
PlayTone(F,38); Wait(40);
PlayTone(C,8); Wait(10);
PlayTone(F,18); Wait(20);
PlayTone(Ges,58); Wait(60);
PlayTone(F,58); Wait(60);
PlayTone(Ges,37); Wait(39);
PlayTone(F,8); Wait(10);
PlayTone(Ges,18); Wait(20);
PlayTone(C8,58); Wait(60);
PlayTone(Ges,58); Wait(60);  

PlayTone(C8,37); Wait(39);
PlayTone(Ges,8); Wait(10);
PlayTone(C8,18); Wait(20);
PlayTone(Des8,58); Wait(60);
PlayTone(Des,58); Wait(60);  
PlayTone(Ges,38); Wait(40);
PlayTone(Des,8); Wait(10);
PlayTone(Ges,32); Wait(34);
PlayTone(C8,120); Wait(120); 
Wait(200);
       
       
   }
}


task main(){      //メインループ

             SetPower(OUT_A,10);  //左右のモータ出力を最大にしておく
             SetPower(OUT_C,10);
SetSensor(S10,SENSOR_LIGHT);    ///
SetSensor(S30,SENSOR_LIGHT);    //センサ1,3を光、2をタッチセンサとして定義
SetSensor(S20,SENSOR_TOUCH);  ///////
int Lmax=0,Rmax=0,Cmax=0,Ltime=0,Rtime=0,Ctime=0;

//  Lmax:  左センサ最高光度  Rmax:  右センサ最高光度  Cmax:  Lmax,Rmaxの平均(光源が真正面に来たときの予測値)
//  Ltime:  左センサが最大になったときの時間  Rtime:  右センサが ” ”
//  Ctime:  Ltime,Rtimeの平均(光源が真正面に来たときの予測時間

Wait(70);
     ClearTimer(0);                         //タイマー0初期化
     while(FastTimer(0)<=430){
      
         if(Lmax<=S10){   //左センサが最大になった時間と値を保存
               Lmax=S10;
               Ltime=FastTimer(0);
                            }
                            
         if(Rmax<=S30){   //上に同じ
               Rmax=S30;
               Rtime=FastTimer(0);
                            }
          OnFwd(OUT_A); ///右旋回
          OnRev(OUT_C);   /////////
      
                                          }
          Off(OUT_A+OUT_C);
          Wait(30);
      Ctime=430 - (Rtime+Ltime)/2;  //ライトが正面に来たときの時間を計算
          OnFwd(OUT_C);  /////
          OnRev(OUT_A);  //ライトの方向を向く
          Wait(Ctime);        ///////////
          Off(OUT_A+OUT_C);
          Cmax=(Lmax+Rmax)/2;  //ライトが正面に来たときの光度を計算
          
     while(true){   //実際の走行プログラム

if(S20==1){   //タッチセンサが押されたら下の処理

  if(S10>Cmax-12 && S30>Cmax-12){  //目の前にライトがあったら停止
       Off(OUT_A+OUT_C);
                               }
   else{                                       //ただの壁ならば少しバックする
       R_R;
       R_L;
       Wait(80);
       Off(OUT_A+OUT_B);
        }

          }



if(S10>Cmax-12 || S30>Cmax-12){   //どちらかのセンサがしきい値以上だったら下の処理
    
        if((S10<=S30+5) && (S10>=S30-5)){ //左右のセンサの差が範囲内だったら左右モータの出力を最大にする
             SetPower(OUT_A,10);
             SetPower(OUT_C,10);
                                                    }
        if(S10<=S30-6){   //右センサの方が大きかったら右モータの出力を下げる
         Off(OUT_C);
            Wait(5);
             SetPower(OUT_A,10);
             SetPower(OUT_C,3);
        Lmax=S10;
        Rmax=S30;
                          }
        if(S30<=S10-6){   //上の処理の逆
         Off(OUT_A);
            Wait(5);
             SetPower(OUT_A,3);
             SetPower(OUT_C,10);
        Lmax=S10;
        Rmax=S30;
                          }
                                 
                                            
            F_R; //左右モータを前進
            F_L; ///////////
            
                                            }
            
         if(S10<=Cmax-12 && S30<=Cmax-12){  //ライトを見失ったら↓
             SetPower(OUT_A,10);
             SetPower(OUT_C,10);
              start music1;              //曲を流しながら↓
until((S10<=S30+6) && (S10>=S30-6) && (S10>Cmax-12 || S30>Cmax-12)){  //ライトが見つかるまで直前のライトの方向に旋回
                         if(Lmax>=Rmax){
                         F_R;R_L;
                                                   }
                         if(Lmax<Rmax){
                         F_L;R_R;
                                                 }
                                                                                                             }
                                      
                                                             }
           stop music1;  //演奏停止
     
                     }

               }

アドバイス等お願いします



トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2006-07-28 (金) 13:12:37