Tomo Suzu / Mbed 2 deprecated soccer_line4EX2

2018/3/20 publish

Dependencies:   DMdriver HMC6352 Ping mbed

main.cpp@0:c2b9f7662334, 2018-03-20 (annotated)

Committer:
Tomo073
Date:
Tue Mar 20 12:57:00 2018 +0000
Revision:
0:c2b9f7662334
Child:
1:456d31e456f1
2018/3/20

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 0:c2b9f7662334 34 HMC6352 mag(p28 , p27); //(SDA,SCL)・要調整
Tomo073 0:c2b9f7662334 35 Driver Driver(p9 , p10); //(SDA,SCL)・要調整
Tomo073 0:c2b9f7662334 36 Ping ping1(p22),ping2(p23),ping3(p21),ping4(p24); //正面,左,右,背後 ・要調整
Tomo073 0:c2b9f7662334 37
Tomo073 0:c2b9f7662334 38 //グローバル変数
Tomo073 0:c2b9f7662334 39 int turn = 3; //回転方向判断変数
Tomo073 0:c2b9f7662334 40 int motorpower[4] = {-60,60,60,-60}; //モーターのパワー・要調整
Tomo073 0:c2b9f7662334 41 float magbasis = -1.0; //地磁気基準値
Tomo073 0:c2b9f7662334 42 float magrange = 20.0; //方向判断の誤差 ・要調整
Tomo073 0:c2b9f7662334 43 float x = 0.0; //進行方向
Tomo073 0:c2b9f7662334 44
Tomo073 0:c2b9f7662334 45 //プロトタイプ宣言
Tomo073 0:c2b9f7662334 46 void turnDicision();
Tomo073 0:c2b9f7662334 47 void turnjudge();
Tomo073 0:c2b9f7662334 48 void balljudge();
Tomo073 0:c2b9f7662334 49 void linejudge();
Tomo073 0:c2b9f7662334 50 void move();
Tomo073 0:c2b9f7662334 51
Tomo073 0:c2b9f7662334 52 int main(){ //メイン
Tomo073 0:c2b9f7662334 53 turnDicision(); //地磁気方向決定
Tomo073 0:c2b9f7662334 54 while(1){
Tomo073 0:c2b9f7662334 55 if(toggle){ //トグルスイッチがオンの時
Tomo073 0:c2b9f7662334 56 turnjudge(); //向き判断
Tomo073 0:c2b9f7662334 57 if(turn == 0){ //正面を向いているとき
Tomo073 0:c2b9f7662334 58 balljudge(); //ボール判断
Tomo073 0:c2b9f7662334 59 linejudge(); //ライン判断
Tomo073 0:c2b9f7662334 60 }
Tomo073 0:c2b9f7662334 61 }else{
Tomo073 0:c2b9f7662334 62 turn = 3; //停止
Tomo073 0:c2b9f7662334 63 }
Tomo073 0:c2b9f7662334 64 move(); //動作
Tomo073 0:c2b9f7662334 65 }
Tomo073 0:c2b9f7662334 66 }
Tomo073 0:c2b9f7662334 67
Tomo073 0:c2b9f7662334 68 void turnDicision(){ //地磁気方向決定
Tomo073 0:c2b9f7662334 69 magbasis = mag.sample()/10.0;
Tomo073 0:c2b9f7662334 70 }
Tomo073 0:c2b9f7662334 71
Tomo073 0:c2b9f7662334 72 void turnjudge(){ //地磁気判断
Tomo073 0:c2b9f7662334 73
Tomo073 0:c2b9f7662334 74 float magdata = -1.0;
Tomo073 0:c2b9f7662334 75 magdata = mag.sample()/10.0 - magbasis;
Tomo073 0:c2b9f7662334 76 if(magdata < 0.0){
Tomo073 0:c2b9f7662334 77 magdata += 360.0;
Tomo073 0:c2b9f7662334 78 }
Tomo073 0:c2b9f7662334 79
Tomo073 0:c2b9f7662334 80 if(magrange < magdata && magdata < 180.0){ //30°～180°
Tomo073 0:c2b9f7662334 81 turn = 1;
Tomo073 0:c2b9f7662334 82 }else if(180.0 <= magdata && magdata < 360.0 - magrange){ //180°～330°
Tomo073 0:c2b9f7662334 83 turn = 2;
Tomo073 0:c2b9f7662334 84 }else{ //330°～30°
Tomo073 0:c2b9f7662334 85 turn = 0;
Tomo073 0:c2b9f7662334 86 }
Tomo073 0:c2b9f7662334 87 }
Tomo073 0:c2b9f7662334 88
Tomo073 0:c2b9f7662334 89 void linejudge(){ //ライン・距離センサによる白線処理
Tomo073 0:c2b9f7662334 90
Tomo073 0:c2b9f7662334 91 int range[4];
Tomo073 0:c2b9f7662334 92
Tomo073 0:c2b9f7662334 93 ping1.Send();
Tomo073 0:c2b9f7662334 94 ping2.Send();
Tomo073 0:c2b9f7662334 95 ping3.Send();
Tomo073 0:c2b9f7662334 96 ping4.Send();
Tomo073 0:c2b9f7662334 97
Tomo073 0:c2b9f7662334 98 wait_ms(30);
Tomo073 0:c2b9f7662334 99
Tomo073 0:c2b9f7662334 100 range[0] = ping1.Read_cm(); //正面
Tomo073 0:c2b9f7662334 101 range[1] = ping2.Read_cm(); //左
Tomo073 0:c2b9f7662334 102 range[2] = ping3.Read_cm(); //右
Tomo073 0:c2b9f7662334 103 range[3] = ping4.Read_cm(); //背後
Tomo073 0:c2b9f7662334 104
Tomo073 0:c2b9f7662334 105 if(line1){ //正面
Tomo073 0:c2b9f7662334 106 if(range[0] > range[3]){
Tomo073 0:c2b9f7662334 107 x = 180.0; //後退
Tomo073 0:c2b9f7662334 108 }else{
Tomo073 0:c2b9f7662334 109 x = 0.0; //前進
Tomo073 0:c2b9f7662334 110 }
Tomo073 0:c2b9f7662334 111 }else if(line2){ //左
Tomo073 0:c2b9f7662334 112 if(range[1] > range[2]){
Tomo073 0:c2b9f7662334 113 x = 90.0; //右へ進む
Tomo073 0:c2b9f7662334 114 }else{
Tomo073 0:c2b9f7662334 115 x = 270.0; //左へ進む
Tomo073 0:c2b9f7662334 116 }
Tomo073 0:c2b9f7662334 117 }else if(line3){ //右
Tomo073 0:c2b9f7662334 118 if(range[1] > range[2]){
Tomo073 0:c2b9f7662334 119 x = 90.0; //右へ進む
Tomo073 0:c2b9f7662334 120 }else{
Tomo073 0:c2b9f7662334 121 x = 270.0; //左へ進む
Tomo073 0:c2b9f7662334 122 }
Tomo073 0:c2b9f7662334 123 }else if(line4){ //背後
Tomo073 0:c2b9f7662334 124 if(range[0] > range[3]){
Tomo073 0:c2b9f7662334 125 x = 180.0; //後退
Tomo073 0:c2b9f7662334 126 }else{
Tomo073 0:c2b9f7662334 127 x = 0.0; //前進
Tomo073 0:c2b9f7662334 128 }
Tomo073 0:c2b9f7662334 129 }else{
Tomo073 0:c2b9f7662334 130 }
Tomo073 0:c2b9f7662334 131 }
Tomo073 0:c2b9f7662334 132
Tomo073 0:c2b9f7662334 133 void balljudge(){ //ボールの位置判断
Tomo073 0:c2b9f7662334 134
Tomo073 0:c2b9f7662334 135 int i;
Tomo073 0:c2b9f7662334 136 float ball[6];
Tomo073 0:c2b9f7662334 137 float max = 0.0;//最大値
Tomo073 0:c2b9f7662334 138 float min = 10.0;//最小値
Tomo073 0:c2b9f7662334 139 float GX,GY;
Tomo073 0:c2b9f7662334 140
Tomo073 0:c2b9f7662334 141 ball[0] = ball1.read(); //正面
Tomo073 0:c2b9f7662334 142 ball[1] = ball2.read(); //左前
Tomo073 0:c2b9f7662334 143 ball[2] = ball3.read(); //右前
Tomo073 0:c2b9f7662334 144 ball[3] = ball4.read(); //左後
Tomo073 0:c2b9f7662334 145 ball[4] = ball5.read(); //右後
Tomo073 0:c2b9f7662334 146 ball[5] = ball6.read(); //背後
Tomo073 0:c2b9f7662334 147
Tomo073 0:c2b9f7662334 148 for (i = 0; i < 6; i++){//最大値判断
Tomo073 0:c2b9f7662334 149 if (max < ball[i]){
Tomo073 0:c2b9f7662334 150 max = ball[i];
Tomo073 0:c2b9f7662334 151 }else if (min > ball[i]){
Tomo073 0:c2b9f7662334 152 min = ball[i];
Tomo073 0:c2b9f7662334 153 }
Tomo073 0:c2b9f7662334 154 }
Tomo073 0:c2b9f7662334 155
Tomo073 0:c2b9f7662334 156 GX = (-ball[1] + ball[2] - ball[3] + ball[4]) * sqrt(3.0)/12.0;
Tomo073 0:c2b9f7662334 157 GY = ((ball[1] + ball[2] - ball[3] - ball[4])/2.0 + ball[0] - ball[5])/6.0;
Tomo073 0:c2b9f7662334 158
Tomo073 0:c2b9f7662334 159 x = -atan2(-GX, GY)*180.0/M_PI; //ボールの角度算出
Tomo073 0:c2b9f7662334 160
Tomo073 0:c2b9f7662334 161 if(45.0 < x && x <= 180.0){ //回り込み・要調整
Tomo073 0:c2b9f7662334 162 if(max > 0.6){ //ボールセンサ閾値・要調整
Tomo073 0:c2b9f7662334 163 x += 45.0; //回り込む角度・要調整
Tomo073 0:c2b9f7662334 164 }else{
Tomo073 0:c2b9f7662334 165 x += 20.0; //回り込む角度・要調整
Tomo073 0:c2b9f7662334 166 }
Tomo073 0:c2b9f7662334 167 }else if(180.0 < x && x < 315.0){ //回り込み・要調整
Tomo073 0:c2b9f7662334 168 if(max > 0.6){ //ボールセンサ閾値・要調整
Tomo073 0:c2b9f7662334 169 x -= 45.0; //回り込む角度・要調整
Tomo073 0:c2b9f7662334 170 }else{
Tomo073 0:c2b9f7662334 171 x -= 20.0; //回り込む角度・要調整
Tomo073 0:c2b9f7662334 172 }
Tomo073 0:c2b9f7662334 173 }
Tomo073 0:c2b9f7662334 174
Tomo073 0:c2b9f7662334 175 if(min < 0.1){ //ボールがないとき・要調整
Tomo073 0:c2b9f7662334 176 x = 180.0; //後退
Tomo073 0:c2b9f7662334 177 }
Tomo073 0:c2b9f7662334 178 }
Tomo073 0:c2b9f7662334 179
Tomo073 0:c2b9f7662334 180 void move(){ //移動・回転方向判断・動作
Tomo073 0:c2b9f7662334 181
Tomo073 0:c2b9f7662334 182 int mp[4];
Tomo073 0:c2b9f7662334 183
Tomo073 0:c2b9f7662334 184 switch(turn){
Tomo073 0:c2b9f7662334 185 case 0: //正面
Tomo073 0:c2b9f7662334 186 mp[0] = sin((x - 315.0)*M_PI/180.0) * motorpower[0];//ch1
Tomo073 0:c2b9f7662334 187 mp[1] = sin((x - 45.0)*M_PI/180.0) * motorpower[1];//ch2
Tomo073 0:c2b9f7662334 188 mp[2] = sin((x - 225.0)*M_PI/180.0) * motorpower[2];//ch3
Tomo073 0:c2b9f7662334 189 mp[3] = sin((x - 135.0)*M_PI/180.0) * motorpower[3];//ch4
Tomo073 0:c2b9f7662334 190 break;
Tomo073 0:c2b9f7662334 191 case 1: //30°～180°
Tomo073 0:c2b9f7662334 192 mp[0] = -motorpower[0]/2;
Tomo073 0:c2b9f7662334 193 mp[1] = -motorpower[1]/2;
Tomo073 0:c2b9f7662334 194 mp[2] = -motorpower[2]/2;
Tomo073 0:c2b9f7662334 195 mp[3] = -motorpower[3]/2;
Tomo073 0:c2b9f7662334 196 break;
Tomo073 0:c2b9f7662334 197 case 2: //180°～330°
Tomo073 0:c2b9f7662334 198 mp[0] = motorpower[0]/2;
Tomo073 0:c2b9f7662334 199 mp[1] = motorpower[1]/2;
Tomo073 0:c2b9f7662334 200 mp[2] = motorpower[2]/2;
Tomo073 0:c2b9f7662334 201 mp[3] = motorpower[3]/2;
Tomo073 0:c2b9f7662334 202 break;
Tomo073 0:c2b9f7662334 203 default: //停止
Tomo073 0:c2b9f7662334 204 mp[0] = 0;
Tomo073 0:c2b9f7662334 205 mp[1] = 0;
Tomo073 0:c2b9f7662334 206 mp[2] = 0;
Tomo073 0:c2b9f7662334 207 mp[3] = 0;
Tomo073 0:c2b9f7662334 208 break;
Tomo073 0:c2b9f7662334 209 }
Tomo073 0:c2b9f7662334 210 Driver.motor(mp[0], mp[1], mp[2], mp[3]);
Tomo073 0:c2b9f7662334 211 }