Tomo Suzu / Mbed 2 deprecated soccer_line4EX2

2018/3/20 publish

Dependencies:   DMdriver HMC6352 Ping mbed

main.cpp@1:456d31e456f1, 2018-03-27 (annotated)

Committer:
Tomo073
Date:
Tue Mar 27 03:26:40 2018 +0000
Revision:
1:456d31e456f1
Parent:
0:c2b9f7662334 
2018/3/27;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tomo073 0:c2b9f7662334 1 /*プログラム設定
Tomo073 0:c2b9f7662334 2 "モーター番号" 　"ボールセンサ番号" "ライン・距離センサ番号"
Tomo073 0:c2b9f7662334 3 ___ __ ___ ___ __ ___ ___ __ ___
Tomo073 0:c2b9f7662334 4 / \ / b1 \ / s1 \
Tomo073 0:c2b9f7662334 5 |ch1 ch2| |b2 b3| | |
Tomo073 0:c2b9f7662334 6 | | | | |s2 s3|
Tomo073 0:c2b9f7662334 7 |ch3 ch4| |b4 b5| | |
Tomo073 0:c2b9f7662334 8 \__________/ \____b6____/ \____s4____/
Tomo073 0:c2b9f7662334 9
Tomo073 0:c2b9f7662334 10 ※１ 上方向が前
Tomo073 0:c2b9f7662334 11 ※２ モーターの正方向は時計回り
Tomo073 0:c2b9f7662334 12 ※３ 進行方向（ｘ）は正面を０度に時計回りに加算
Tomo073 0:c2b9f7662334 13
Tomo073 0:c2b9f7662334 14 動作マニュアル
Tomo073 0:c2b9f7662334 15 ・ピン番号は要確認
Tomo073 0:c2b9f7662334 16 ・起動時とリセット時に地磁気基準方向決定 ⇒ 地磁気は前に固定、コートに戻す際注意
Tomo073 0:c2b9f7662334 17 ・要調整の場所は微調整しておくこと
Tomo073 0:c2b9f7662334 18 */
Tomo073 0:c2b9f7662334 19
Tomo073 0:c2b9f7662334 20 #include "mbed.h"
Tomo073 0:c2b9f7662334 21 #include "Ping.h"
Tomo073 0:c2b9f7662334 22 #include "HMC6352.h"
Tomo073 0:c2b9f7662334 23 #include "DMdriver.h"
Tomo073 0:c2b9f7662334 24 #include "math.h"
Tomo073 0:c2b9f7662334 25
Tomo073 0:c2b9f7662334 26 #ifndef M_PI
Tomo073 0:c2b9f7662334 27 #define M_PI 3.14159265358979323846
Tomo073 0:c2b9f7662334 28 #endif
Tomo073 0:c2b9f7662334 29
Tomo073 0:c2b9f7662334 30 //使用するピン
Tomo073 0:c2b9f7662334 31 AnalogIn ball1(p15), ball2(p16), ball3(p17), ball4(p18), ball5(p19), ball6(p20); //正面,左前,右前,左後,右後,背後 ・要調整
Tomo073 0:c2b9f7662334 32 DigitalIn line1(p11), line2(p12), line3(p13), line4(p14); //正面,左,右,背後 ・要調整
Tomo073 0:c2b9f7662334 33 DigitalIn toggle(p7); //・要調整
Tomo073 1:456d31e456f1 34 DigitalOut myled1(LED1),myled2(LED2),myled3(LED3),myled4(LED4);
Tomo073 0:c2b9f7662334 35 HMC6352 mag(p28 , p27); //(SDA,SCL)・要調整
Tomo073 0:c2b9f7662334 36 Driver Driver(p9 , p10); //(SDA,SCL)・要調整
Tomo073 0:c2b9f7662334 37 Ping ping1(p22),ping2(p23),ping3(p21),ping4(p24); //正面,左,右,背後 ・要調整
Tomo073 0:c2b9f7662334 38
Tomo073 0:c2b9f7662334 39 //グローバル変数
Tomo073 0:c2b9f7662334 40 int turn = 3; //回転方向判断変数
Tomo073 0:c2b9f7662334 41 int motorpower[4] = {-60,60,60,-60}; //モーターのパワー・要調整
Tomo073 0:c2b9f7662334 42 float magbasis = -1.0; //地磁気基準値
Tomo073 0:c2b9f7662334 43 float magrange = 20.0; //方向判断の誤差 ・要調整
Tomo073 0:c2b9f7662334 44 float x = 0.0; //進行方向
Tomo073 0:c2b9f7662334 45
Tomo073 0:c2b9f7662334 46 //プロトタイプ宣言
Tomo073 0:c2b9f7662334 47 void turnDicision();
Tomo073 0:c2b9f7662334 48 void turnjudge();
Tomo073 0:c2b9f7662334 49 void balljudge();
Tomo073 0:c2b9f7662334 50 void linejudge();
Tomo073 0:c2b9f7662334 51 void move();
Tomo073 0:c2b9f7662334 52
Tomo073 0:c2b9f7662334 53 int main(){ //メイン
Tomo073 0:c2b9f7662334 54 turnDicision(); //地磁気方向決定
Tomo073 0:c2b9f7662334 55 while(1){
Tomo073 0:c2b9f7662334 56 if(toggle){ //トグルスイッチがオンの時
Tomo073 0:c2b9f7662334 57 turnjudge(); //向き判断
Tomo073 0:c2b9f7662334 58 if(turn == 0){ //正面を向いているとき
Tomo073 0:c2b9f7662334 59 balljudge(); //ボール判断
Tomo073 0:c2b9f7662334 60 linejudge(); //ライン判断
Tomo073 0:c2b9f7662334 61 }
Tomo073 0:c2b9f7662334 62 }else{
Tomo073 0:c2b9f7662334 63 turn = 3; //停止
Tomo073 0:c2b9f7662334 64 }
Tomo073 0:c2b9f7662334 65 move(); //動作
Tomo073 0:c2b9f7662334 66 }
Tomo073 0:c2b9f7662334 67 }
Tomo073 0:c2b9f7662334 68
Tomo073 0:c2b9f7662334 69 void turnDicision(){ //地磁気方向決定
Tomo073 0:c2b9f7662334 70 magbasis = mag.sample()/10.0;
Tomo073 0:c2b9f7662334 71 }
Tomo073 0:c2b9f7662334 72
Tomo073 0:c2b9f7662334 73 void turnjudge(){ //地磁気判断
Tomo073 0:c2b9f7662334 74
Tomo073 0:c2b9f7662334 75 float magdata = -1.0;
Tomo073 0:c2b9f7662334 76 magdata = mag.sample()/10.0 - magbasis;
Tomo073 0:c2b9f7662334 77 if(magdata < 0.0){
Tomo073 0:c2b9f7662334 78 magdata += 360.0;
Tomo073 0:c2b9f7662334 79 }
Tomo073 0:c2b9f7662334 80
Tomo073 0:c2b9f7662334 81 if(magrange < magdata && magdata < 180.0){ //30°～180°
Tomo073 0:c2b9f7662334 82 turn = 1;
Tomo073 0:c2b9f7662334 83 }else if(180.0 <= magdata && magdata < 360.0 - magrange){ //180°～330°
Tomo073 0:c2b9f7662334 84 turn = 2;
Tomo073 0:c2b9f7662334 85 }else{ //330°～30°
Tomo073 0:c2b9f7662334 86 turn = 0;
Tomo073 0:c2b9f7662334 87 }
Tomo073 0:c2b9f7662334 88 }
Tomo073 0:c2b9f7662334 89
Tomo073 1:456d31e456f1 90 void balljudge(){ //ボールの位置判断
Tomo073 1:456d31e456f1 91
Tomo073 1:456d31e456f1 92 int i;
Tomo073 1:456d31e456f1 93 float ball[6];
Tomo073 1:456d31e456f1 94 float max = 0.0;//最大値
Tomo073 1:456d31e456f1 95 float min = 10.0;//最小値
Tomo073 1:456d31e456f1 96 float GX,GY;
Tomo073 1:456d31e456f1 97
Tomo073 1:456d31e456f1 98 ball[0] = ball1.read(); //正面
Tomo073 1:456d31e456f1 99 ball[1] = ball2.read(); //左前
Tomo073 1:456d31e456f1 100 ball[2] = ball3.read(); //右前
Tomo073 1:456d31e456f1 101 ball[3] = ball4.read(); //左後
Tomo073 1:456d31e456f1 102 ball[4] = ball5.read(); //右後
Tomo073 1:456d31e456f1 103 ball[5] = ball6.read(); //背後
Tomo073 1:456d31e456f1 104
Tomo073 1:456d31e456f1 105 for (i = 0; i < 6; i++){//最大値判断
Tomo073 1:456d31e456f1 106 if (max < ball[i]){
Tomo073 1:456d31e456f1 107 max = ball[i];
Tomo073 1:456d31e456f1 108 }else if (min > ball[i]){
Tomo073 1:456d31e456f1 109 min = ball[i];
Tomo073 1:456d31e456f1 110 }
Tomo073 1:456d31e456f1 111 }
Tomo073 1:456d31e456f1 112
Tomo073 1:456d31e456f1 113 GX = (-ball[1] + ball[2] - ball[3] + ball[4]) * sqrt(3.0)/12.0;
Tomo073 1:456d31e456f1 114 GY = ((ball[1] + ball[2] - ball[3] - ball[4])/2.0 + ball[0] - ball[5])/6.0;
Tomo073 1:456d31e456f1 115
Tomo073 1:456d31e456f1 116 x = -atan2(-GX, GY)*180.0/M_PI; //ボールの角度算出
Tomo073 1:456d31e456f1 117
Tomo073 1:456d31e456f1 118 if(x < 10.0 || 350.0 < x){
Tomo073 1:456d31e456f1 119 x = 0.0;
Tomo073 1:456d31e456f1 120 }else if(60.0 < x && x <= 180.0){ //回り込み・要調整
Tomo073 1:456d31e456f1 121 if(max > 0.6){ //ボールセンサ閾値・要調整
Tomo073 1:456d31e456f1 122 x += 40.0; //回り込む角度・要調整
Tomo073 1:456d31e456f1 123 }else{
Tomo073 1:456d31e456f1 124 x += 20.0; //回り込む角度・要調整
Tomo073 1:456d31e456f1 125 }
Tomo073 1:456d31e456f1 126 }else if(180.0 < x && x < 300.0){ //回り込み・要調整
Tomo073 1:456d31e456f1 127 if(max > 0.6){ //ボールセンサ閾値・要調整
Tomo073 1:456d31e456f1 128 x -= 40.0; //回り込む角度・要調整
Tomo073 1:456d31e456f1 129 }else{
Tomo073 1:456d31e456f1 130 x -= 20.0; //回り込む角度・要調整
Tomo073 1:456d31e456f1 131 }
Tomo073 1:456d31e456f1 132 }
Tomo073 1:456d31e456f1 133
Tomo073 1:456d31e456f1 134 /*if(min < 0.1){ //ボールがないとき・要調整
Tomo073 1:456d31e456f1 135 x = 180.0; //後退
Tomo073 1:456d31e456f1 136 }*/
Tomo073 1:456d31e456f1 137 }
Tomo073 1:456d31e456f1 138
Tomo073 0:c2b9f7662334 139 void linejudge(){ //ライン・距離センサによる白線処理
Tomo073 0:c2b9f7662334 140
Tomo073 0:c2b9f7662334 141 int range[4];
Tomo073 0:c2b9f7662334 142
Tomo073 0:c2b9f7662334 143 ping1.Send();
Tomo073 0:c2b9f7662334 144 ping2.Send();
Tomo073 0:c2b9f7662334 145 ping3.Send();
Tomo073 0:c2b9f7662334 146 ping4.Send();
Tomo073 0:c2b9f7662334 147
Tomo073 0:c2b9f7662334 148 wait_ms(30);
Tomo073 0:c2b9f7662334 149
Tomo073 0:c2b9f7662334 150 range[0] = ping1.Read_cm(); //正面
Tomo073 0:c2b9f7662334 151 range[1] = ping2.Read_cm(); //左
Tomo073 0:c2b9f7662334 152 range[2] = ping3.Read_cm(); //右
Tomo073 0:c2b9f7662334 153 range[3] = ping4.Read_cm(); //背後
Tomo073 0:c2b9f7662334 154
Tomo073 0:c2b9f7662334 155 if(line1){ //正面
Tomo073 0:c2b9f7662334 156 if(range[0] > range[3]){
Tomo073 0:c2b9f7662334 157 x = 180.0; //後退
Tomo073 0:c2b9f7662334 158 }else{
Tomo073 0:c2b9f7662334 159 x = 0.0; //前進
Tomo073 0:c2b9f7662334 160 }
Tomo073 0:c2b9f7662334 161 }else if(line2){ //左
Tomo073 0:c2b9f7662334 162 if(range[1] > range[2]){
Tomo073 0:c2b9f7662334 163 x = 90.0; //右へ進む
Tomo073 0:c2b9f7662334 164 }else{
Tomo073 0:c2b9f7662334 165 x = 270.0; //左へ進む
Tomo073 0:c2b9f7662334 166 }
Tomo073 0:c2b9f7662334 167 }else if(line3){ //右
Tomo073 0:c2b9f7662334 168 if(range[1] > range[2]){
Tomo073 0:c2b9f7662334 169 x = 90.0; //右へ進む
Tomo073 0:c2b9f7662334 170 }else{
Tomo073 0:c2b9f7662334 171 x = 270.0; //左へ進む
Tomo073 0:c2b9f7662334 172 }
Tomo073 0:c2b9f7662334 173 }else if(line4){ //背後
Tomo073 0:c2b9f7662334 174 if(range[0] > range[3]){
Tomo073 0:c2b9f7662334 175 x = 180.0; //後退
Tomo073 0:c2b9f7662334 176 }else{
Tomo073 0:c2b9f7662334 177 x = 0.0; //前進
Tomo073 0:c2b9f7662334 178 }
Tomo073 1:456d31e456f1 179
Tomo073 0:c2b9f7662334 180 }
Tomo073 0:c2b9f7662334 181
Tomo073 0:c2b9f7662334 182 void move(){ //移動・回転方向判断・動作
Tomo073 0:c2b9f7662334 183
Tomo073 0:c2b9f7662334 184 int mp[4];
Tomo073 0:c2b9f7662334 185
Tomo073 0:c2b9f7662334 186 switch(turn){
Tomo073 0:c2b9f7662334 187 case 0: //正面
Tomo073 0:c2b9f7662334 188 mp[0] = sin((x - 315.0)*M_PI/180.0) * motorpower[0];//ch1
Tomo073 0:c2b9f7662334 189 mp[1] = sin((x - 45.0)*M_PI/180.0) * motorpower[1];//ch2
Tomo073 0:c2b9f7662334 190 mp[2] = sin((x - 225.0)*M_PI/180.0) * motorpower[2];//ch3
Tomo073 0:c2b9f7662334 191 mp[3] = sin((x - 135.0)*M_PI/180.0) * motorpower[3];//ch4
Tomo073 0:c2b9f7662334 192 break;
Tomo073 0:c2b9f7662334 193 case 1: //30°～180°
Tomo073 0:c2b9f7662334 194 mp[0] = -motorpower[0]/2;
Tomo073 0:c2b9f7662334 195 mp[1] = -motorpower[1]/2;
Tomo073 0:c2b9f7662334 196 mp[2] = -motorpower[2]/2;
Tomo073 0:c2b9f7662334 197 mp[3] = -motorpower[3]/2;
Tomo073 0:c2b9f7662334 198 break;
Tomo073 0:c2b9f7662334 199 case 2: //180°～330°
Tomo073 0:c2b9f7662334 200 mp[0] = motorpower[0]/2;
Tomo073 0:c2b9f7662334 201 mp[1] = motorpower[1]/2;
Tomo073 0:c2b9f7662334 202 mp[2] = motorpower[2]/2;
Tomo073 0:c2b9f7662334 203 mp[3] = motorpower[3]/2;
Tomo073 0:c2b9f7662334 204 break;
Tomo073 0:c2b9f7662334 205 default: //停止
Tomo073 0:c2b9f7662334 206 mp[0] = 0;
Tomo073 0:c2b9f7662334 207 mp[1] = 0;
Tomo073 0:c2b9f7662334 208 mp[2] = 0;
Tomo073 0:c2b9f7662334 209 mp[3] = 0;
Tomo073 0:c2b9f7662334 210 break;
Tomo073 0:c2b9f7662334 211 }
Tomo073 0:c2b9f7662334 212 Driver.motor(mp[0], mp[1], mp[2], mp[3]);
Tomo073 0:c2b9f7662334 213 }