Revision 7:7a0aee1477d9, committed 2015-01-10

Comitter:

ryuna

Date:

Sat Jan 10 13:40:02 2015 +0000

Parent:

6:c2c31bc971ad

Commit message:

??????????????????????????; wraparound.cpp??????????????

Changed in this revision

diff -r c2c31bc971ad -r 7a0aee1477d9 main.cpp
--- a/main.cpp	Fri Jan 09 06:59:18 2015 +0000
+++ b/main.cpp	Sat Jan 10 13:40:02 2015 +0000
@@ -204,11 +204,10 @@
     if(data_r[2]>data_l[2]){       
         return data_r[1];
         
-    }else{ 
-        return data_l[1];
-        
     }
     
+    return data_l[1];
+    
 }
 unsigned int IrReceiveSM(){
     /*値解析版　一番大きい値とその位置を返す
@@ -275,7 +274,16 @@
     
 }
 
-
+void PingReceiveFB(char ping[]){//ping[0]==FRONT,ping[1]==BACK
+    char ReadSelect[1] = {READ_PING};
+    bool val;
+    val = Sensor.write(ADDRESS_L|0, ReadSelect , 1);
+    Led[2] = val;
+    val = Sensor.read(ADDRESS_L|1, ping, 2);
+    Led[2] = !val;
+    
+    
+}
 
 
 void rotate(unsigned int times, bool dir){
@@ -394,12 +402,99 @@
 void ControlRobo2(int *CompassDef)//RightFront
 {
     
-
-
+    int Compass;
+    char Ir[2] ={0};//1, 1,2位, 1位の大きさ
+    char ir_num,line;//わかりやすい名前
+    
+    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+    if(!((Compass>=150)&&(Compass<=210))){
+        while(!((Compass>=150)&&(Compass<=210))){
+            move(20,-20);//適当な回転。
+            Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+        }
+        move(0,0);
+        return;
+    }
+    Step.GoHome();
+    /*irデータ取得 1,2位の場所、一位の距離*/
+    if(!(Ir[0]%13 ==2)){
+        return;
+    }
+    Step.BusyWait(0);
+    
+    if(Ir[1]>=100){//近い場合
+        //2位を含んだ計算
+        /*motorset*/
+        /*stepset*/
+        return;    
+    }
+    
+    /*2位を含んだ計算*/
+    /*計算された分stepmove*/
+    Step.BusyWait(0);
+    move(20,20);
+    
+    
+    /*line,ir get*/
+    do{
+        if(line){
+            move(0,0);
+            return;
+        }
+    
+    /*line,ir get*/
+    }while(ir_num == 2);
+    
+    move(0,0);
+    
+    
 
 }
 void ControlRobo3(int *CompassDef)//RightFront
-{}
+{
+    int Compass;
+    char Ir[2] ={0};//1, 1,2位, 1位の大きさ
+    char ir_num,line;//わかりやすい名前
+    
+    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+    if(!((Compass>=150)&&(Compass<=210))){
+        while(!((Compass>=150)&&(Compass<=210))){
+            move(20,-20);//適当な回転。
+            Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+        }
+        move(0,0);
+        return;
+    }
+    Step.GoHome();
+    /*irデータ取得 1,2位の場所、一位の距離*/
+    if(!(Ir[0]%13 ==3)){
+        return;
+    }
+    Step.BusyWait(0);
+    
+    if(Ir[1] >= 100){
+        /*2位を含めた計算*/
+        /*move()*/
+        /*Stepset*/
+        return;
+    }
+    /*2位を含めた計算*/
+    /*Stepsetting 距離等により角度が変わる*/
+    Step.BusyWait(0);
+    move(-20,-20);
+    /*ir,line check*/
+    do{
+        if(line){
+            move(0,0);
+            return;
+        }
+        /*line,ir check*/
+    }while(ir_num == 3);
+    
+    move(0,0);
+    
+
+}
 void ControlRobo4(int *CompassDef)//Right
 {
     int a = 50;
@@ -435,9 +530,75 @@
 
 }
 void ControlRobo5(int *CompassDef)//RightBack
-{}
+{
+    int Compass;
+    char Ir[2] ={0};//1, 1,2位, 1位の大きさ
+    char ir_num,line;//わかりやすい名前
+    
+    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+    if(!((Compass>=150)&&(Compass<=210))){
+        while(!((Compass>=150)&&(Compass<=210))){
+            move(20,-20);//適当な回転。
+            Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+        }
+        move(0,0);
+        return;
+    }
+    Step.GoHome();
+    /*irデータ取得 1,2位の場所、一位の距離*/
+    if(!(Ir[0]%13 == 5)){
+        return;
+    }
+    Step.BusyWait(0);
+    
+    if(Ir[1] >=100){//近い
+        /*計算*/
+        /*Step.Move()回転開始位置まで移動*/
+        Step.BusyWait(0);
+        move(-20,-20);
+        /*Step.setpaaa
+          Step.move();-180+初期位置
+         */
+        /*ir,linecheck*/
+        do{
+            if(line){
+                move(0,0);
+                return;
+            }
+            if(!(ir_num >1 && ir_num <=5)){
+                move(0,0);
+                return;
+            }
+            /*ir,line check*/
+        }while(1);
+    }
+    //遠い
+    
+        /*計算*/
+        move(-20,-20);
+        /*Step.setpaaa
+          Step.move();
+         */
+        /*ir,linecheck*/
+        do{
+            if(line){
+                move(0,0);
+                return;
+            }
+            if(!(ir_num >1 && ir_num <=5)){
+                move(0,0);
+                return;
+            }
+            /*ir,line check*/
+        }while(1);
+
+
+}
 void ControlRobo6(int *CompassDef)//BackRight
-{}
+{
+    
+
+}
 void ControlRobo7(int *CompassDef)//Back
 {
     /*****
@@ -466,7 +627,6 @@
     /*半円なのであらかじめステッピングを回転させる必要がある*/
     /*モーター設定*/
     /*ステッピング設定*/
-    return;
     
     
     }
@@ -480,10 +640,31 @@
 {}
 void GoHome(int *CompassDef)//Ball is not found.
 {
-        
+    /*line検知をしないバージョン*/
+    int Compass;
+    char ping[2] = {0};
     
+    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+    if(!((Compass>=150)&&(Compass<=210))){
+        while(!((Compass>=150)&&(Compass<=210))){
+            move(20,-20);//適当な回転。
+            Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
+        }
+        /*return;してもいいかもしれない*/
+    }
+    Step.GoHome();
+    Step.BusyWait(0);
+     
+    PingReceiveFB(ping);
+    if(ping[1] >=60){//後ろからの距離60cm
+        while(ping[1] >=60){
+            move(-20,-20);
+            PingReceiveFB(ping);
+        }
+    } 
+        
+    move(0,0);//stop
     
-
 }
 
 
@@ -532,7 +713,7 @@
     move(0,0);//停止
 
     Step.Resets();
-    Step.ReleseSW(0,1);//記憶がない
+    Step.ReleseSW(0,1);
 
     hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     *compass_def = (hmc6352.sample() / 10);
@@ -551,7 +732,7 @@
      *
      *switch割り当て
      *1.コンパスのキャリブレーション実行スイッチ
-     *2.キッカーのキック
+     *2.キッカーのキック(check用)
      *3,4.自由
      *5.StartSw
      */
@@ -648,12 +829,12 @@
         Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
         if(Compass == 3) break;//前を向いたら抜ける。
     }
-    while(1) {
+    while(0) {
         
         Line = LineReceive();
         
         if(Line){ //ラインを検知していないか
-            LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
+            LineIr = Line & (IrReceiveS()/3 + 1);//計(lineとirの方位を合わせる。
             while(LineIr){
                 move(0,0);//
                 Led[1] = Led[2] = Led[3] = 1;  
@@ -675,6 +856,37 @@
         (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。
         
         //wait_ms(0);
+    }
+    
+    while(1){
+    /*デモプログラム
+     *
+     *ぐるぐる
+     *
+     *
+     */    
+        move(30,30);
+        
+        wait(1);
+        
+        move(-30,-30);
+        move(0,0);
+        
+        Step.Run(1,1200);
+        move(10,10);
+        wait(1.5);
+        
+        move(0,0);
+        Step.SoftStop();
+        
+        wait(0.5);
+        
+        Step.GoHome();
+        
+        wait(1.5);
+        
+    
         
     }
+    
 }

diff -r c2c31bc971ad -r 7a0aee1477d9 wraparound.cpp
--- a/wraparound.cpp	Fri Jan 09 06:59:18 2015 +0000
+++ b/wraparound.cpp	Sat Jan 10 13:40:02 2015 +0000
@@ -15,12 +15,28 @@
 Ir2//irの二番目に値が大きいもの
 
 
+以上は割と序盤に考えたことで適当なこと言ってる。
 
-思考1　フローチャートを用いる。 12/26~ 
+
+1　フローチャートを用いる。 12/26~ 
 
 露骨にオートマトンのプログラムを参考にしている。
 
 
+もしかして途中までの計算まとめられる
+ir分岐後の計算がほとんど同じ
+動きが他のものと違う後ろと前以外を先に関数でどっかに飛ばす
+if(ir == 1 || ir == 7){
+ (*ControlRobo[ir/7])(&CompassDef); //これで0,1に分けれるはず
+ continue;
+}
+
+この後ほかのやつは似たような処理をしたあと　
+ それぞれに分岐するとか考えられる。
+ 
+    逆にこれによる害は何か。
+    
+    
 
 
 
@@ -37,7 +53,7 @@
 
 irは＋-30度まで読めるが,距離が遠い場所ほど減衰
 
-ボールがないとき12
+ボールがないときは12
           
               *1     
       *0              *2