Tomo Suzu / Mbed 2 deprecated soccer_line4EX2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   DMdriver HMC6352 Ping mbed

Files at this revision

API Documentation at this revision

Revision 0:c2b9f7662334, committed 2018-03-20

Comitter:
Tomo073
Date:
Tue Mar 20 12:57:00 2018 +0000
Child:
1:456d31e456f1
Commit message:
2018/3/20

Changed in this revision

DMdriver.lib Show annotated file Show diff for this revision Revisions of this file
HMC6352.lib Show annotated file Show diff for this revision Revisions of this file
Ping.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DMdriver.lib	Tue Mar 20 12:57:00 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Tomo073/code/DMdriver/#d7d8bd043212

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Tue Mar 20 12:57:00 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Tomo073/code/HMC6352/#e009fb4791c0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Ping.lib	Tue Mar 20 12:57:00 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Tomo073/code/Ping/#a51e9c49571a

--- /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]);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Mar 20 12:57:00 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/aa5281ff4a02
\ No newline at end of file