Diff: main.cpp

Revision:

0:c2b9f7662334

Child:

1:456d31e456f1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 20 12:57:00 2018 +0000
@@ -0,0 +1,211 @@
+/*プログラム設定
+    "モーター番号"    　"ボールセンサ番号"    "ライン・距離センサ番号"
+     ___ __ ___          ___ __ ___            ___ __ ___
+    /          \        /    b1    \          /    s1    \
+    |ch1    ch2|        |b2      b3|          |          |
+    |          |        |          |          |s2      s3|
+    |ch3    ch4|        |b4      b5|          |          |
+    \__________/        \____b6____/          \____s4____/
+    
+※１ 上方向が前
+※２ モーターの正方向は時計回り
+※３ 進行方向（ｘ）は正面を０度に時計回りに加算
+
+  動作マニュアル
+・ピン番号は要確認
+・起動時とリセット時に地磁気基準方向決定 ⇒ 地磁気は前に固定、コートに戻す際注意
+・要調整の場所は微調整しておくこと
+*/
+
+#include "mbed.h"
+#include "Ping.h"
+#include "HMC6352.h"
+#include "DMdriver.h"
+#include "math.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+//使用するピン
+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); //・要調整
+HMC6352 mag(p28 , p27); //(SDA,SCL)・要調整
+Driver Driver(p9 , p10); //(SDA,SCL)・要調整
+Ping ping1(p22),ping2(p23),ping3(p21),ping4(p24); //正面,左,右,背後 ・要調整
+
+//グローバル変数
+int turn = 3; //回転方向判断変数
+int motorpower[4] = {-60,60,60,-60}; //モーターのパワー・要調整
+float magbasis = -1.0; //地磁気基準値
+float magrange = 20.0; //方向判断の誤差 ・要調整
+float x = 0.0; //進行方向
+
+//プロトタイプ宣言
+void turnDicision(); 
+void turnjudge();
+void balljudge();
+void linejudge();
+void move();
+
+int main(){ //メイン
+    turnDicision(); //地磁気方向決定
+    while(1){
+        if(toggle){  //トグルスイッチがオンの時
+           turnjudge(); //向き判断            
+           if(turn == 0){ //正面を向いているとき
+              balljudge(); //ボール判断
+              linejudge(); //ライン判断 
+           }  
+        }else{
+            turn = 3; //停止
+        }
+        move(); //動作
+    }
+}
+
+void turnDicision(){ //地磁気方向決定
+    magbasis = mag.sample()/10.0;
+}
+
+void turnjudge(){ //地磁気判断
+    
+    float magdata = -1.0;
+    magdata = mag.sample()/10.0 - magbasis;
+      if(magdata < 0.0){
+          magdata += 360.0;
+      }
+    
+    if(magrange < magdata && magdata < 180.0){ //30°～180°
+       turn = 1;
+    }else if(180.0 <= magdata && magdata < 360.0 - magrange){ //180°～330°
+       turn = 2;
+    }else{ //330°～30°
+       turn = 0;
+    }
+}
+
+void linejudge(){ //ライン・距離センサによる白線処理
+     
+     int range[4];
+     
+     ping1.Send(); 
+     ping2.Send();  
+     ping3.Send();  
+     ping4.Send();  
+           
+     wait_ms(30);
+        
+     range[0] = ping1.Read_cm(); //正面 
+     range[1] = ping2.Read_cm(); //左 
+     range[2] = ping3.Read_cm(); //右 
+     range[3] = ping4.Read_cm(); //背後
+        
+     if(line1){ //正面
+      if(range[0] > range[3]){
+          x = 180.0; //後退
+      }else{
+          x = 0.0; //前進
+      }
+    }else if(line2){ //左
+      if(range[1] > range[2]){
+          x = 90.0; //右へ進む
+      }else{
+          x = 270.0; //左へ進む
+      }
+    }else if(line3){ //右
+      if(range[1] > range[2]){
+          x = 90.0; //右へ進む
+      }else{
+          x = 270.0; //左へ進む
+      }
+    }else if(line4){ //背後
+      if(range[0] > range[3]){
+          x = 180.0; //後退
+      }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];
+    
+    switch(turn){
+        case 0: //正面   
+        mp[0] = sin((x - 315.0)*M_PI/180.0) * motorpower[0];//ch1
+        mp[1] = sin((x - 45.0)*M_PI/180.0) * motorpower[1];//ch2
+        mp[2] = sin((x - 225.0)*M_PI/180.0) * motorpower[2];//ch3
+        mp[3] = sin((x - 135.0)*M_PI/180.0) * motorpower[3];//ch4
+        break;
+        case 1: //30°～180°
+        mp[0] = -motorpower[0]/2;
+        mp[1] = -motorpower[1]/2;
+        mp[2] = -motorpower[2]/2;
+        mp[3] = -motorpower[3]/2;
+        break;
+        case 2: //180°～330°
+        mp[0] = motorpower[0]/2;
+        mp[1] = motorpower[1]/2;
+        mp[2] = motorpower[2]/2;
+        mp[3] = motorpower[3]/2;
+        break;
+        default: //停止
+        mp[0] = 0;
+        mp[1] = 0;
+        mp[2] = 0;
+        mp[3] = 0;
+        break;
+     }
+    Driver.motor(mp[0], mp[1], mp[2], mp[3]);
+}