Diff: main.cpp

Revision:

1:456d31e456f1

Parent:

0:c2b9f7662334

--- a/main.cpp	Tue Mar 20 12:57:00 2018 +0000
+++ b/main.cpp	Tue Mar 27 03:26:40 2018 +0000
@@ -31,6 +31,7 @@
 AnalogIn ball1(p15), ball2(p16), ball3(p17), ball4(p18), ball5(p19), ball6(p20); //正面,左前,右前,左後,右後,背後 ・要調整
 DigitalIn line1(p11), line2(p12), line3(p13), line4(p14); //正面,左,右,背後 ・要調整
 DigitalIn toggle(p7); //・要調整
+DigitalOut myled1(LED1),myled2(LED2),myled3(LED3),myled4(LED4);
 HMC6352 mag(p28 , p27); //(SDA,SCL)・要調整
 Driver Driver(p9 , p10); //(SDA,SCL)・要調整
 Ping ping1(p22),ping2(p23),ping3(p21),ping4(p24); //正面,左,右,背後 ・要調整
@@ -86,6 +87,55 @@
     }
 }
 
+void balljudge(){ //ボールの位置判断
+    
+    int i;
+    float ball[6];
+    float max = 0.0;//最大値
+    float min = 10.0;//最小値
+    float GX,GY;
+
+    ball[0] = ball1.read(); //正面 
+    ball[1] = ball2.read(); //左前 
+    ball[2] = ball3.read(); //右前 
+    ball[3] = ball4.read(); //左後 
+    ball[4] = ball5.read(); //右後 
+    ball[5] = ball6.read(); //背後 
+    
+    for (i = 0; i < 6; i++){//最大値判断
+        if (max < ball[i]){
+           max = ball[i];
+        }else if (min > ball[i]){
+           min = ball[i];
+        }
+    }
+    
+    GX = (-ball[1] + ball[2] - ball[3] + ball[4]) * sqrt(3.0)/12.0;
+    GY = ((ball[1] + ball[2] - ball[3] - ball[4])/2.0 + ball[0] - ball[5])/6.0;
+    
+    x = -atan2(-GX, GY)*180.0/M_PI; //ボールの角度算出
+    
+    if(x < 10.0 || 350.0 < x){
+         x = 0.0;
+    }else if(60.0 < x && x <= 180.0){ //回り込み・要調整
+      if(max > 0.6){ //ボールセンサ閾値・要調整
+         x += 40.0; //回り込む角度・要調整
+      }else{
+         x += 20.0; //回り込む角度・要調整
+      }
+    }else if(180.0 < x && x < 300.0){ //回り込み・要調整
+      if(max > 0.6){ //ボールセンサ閾値・要調整
+         x -= 40.0; //回り込む角度・要調整
+      }else{
+         x -= 20.0; //回り込む角度・要調整
+      }
+    }
+    
+    /*if(min < 0.1){ //ボールがないとき・要調整
+       x = 180.0; //後退
+    }*/
+}
+
 void linejudge(){ //ライン・距離センサによる白線処理
      
      int range[4];
@@ -126,57 +176,9 @@
       }else{
           x = 0.0; //前進
       }
-    }else{
-    }
+      
 }    
 
-void balljudge(){ //ボールの位置判断
-    
-    int i;
-    float ball[6];
-    float max = 0.0;//最大値
-    float min = 10.0;//最小値
-    float GX,GY;
-
-    ball[0] = ball1.read(); //正面 
-    ball[1] = ball2.read(); //左前 
-    ball[2] = ball3.read(); //右前 
-    ball[3] = ball4.read(); //左後 
-    ball[4] = ball5.read(); //右後 
-    ball[5] = ball6.read(); //背後 
-    
-    for (i = 0; i < 6; i++){//最大値判断
-        if (max < ball[i]){
-           max = ball[i];
-        }else if (min > ball[i]){
-           min = ball[i];
-        }
-    }
-    
-    GX = (-ball[1] + ball[2] - ball[3] + ball[4]) * sqrt(3.0)/12.0;
-    GY = ((ball[1] + ball[2] - ball[3] - ball[4])/2.0 + ball[0] - ball[5])/6.0;
-    
-    x = -atan2(-GX, GY)*180.0/M_PI; //ボールの角度算出
-    
-    if(45.0 < x && x <= 180.0){ //回り込み・要調整
-      if(max > 0.6){ //ボールセンサ閾値・要調整
-         x += 45.0; //回り込む角度・要調整
-      }else{
-         x += 20.0; //回り込む角度・要調整
-      }
-    }else if(180.0 < x && x < 315.0){ //回り込み・要調整
-      if(max > 0.6){ //ボールセンサ閾値・要調整
-         x -= 45.0; //回り込む角度・要調整
-      }else{
-         x -= 20.0; //回り込む角度・要調整
-      }
-    }
-    
-    if(min < 0.1){ //ボールがないとき・要調整
-       x = 180.0; //後退
-    }
-}
-
 void move(){ //移動・回転方向判断・動作
     
     int mp[4];