CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver2

2

Dependencies:   AQM0802A HMC6352 PID Servo mbed

main.cpp@0:dfe81bdcb486, 2015-02-13 (annotated)

Committer:
ryuna
Date:
Fri Feb 13 10:11:49 2015 +0000
Revision:
0:dfe81bdcb486
Child:
1:438016436d16
Child:
3:0c994b3a882e
i2c??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:dfe81bdcb486 1 /***********************************
ryuna 0:dfe81bdcb486 2 *RoboCupJunior Soccer B Open 2014
ryuna 0:dfe81bdcb486 3 *Koshinnestu progrum
ryuna 0:dfe81bdcb486 4 *
ryuna 0:dfe81bdcb486 5 *　
ryuna 0:dfe81bdcb486 6 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
ryuna 0:dfe81bdcb486 7 *
ryuna 0:dfe81bdcb486 8 *　MotorDriverにmaxonに命令
ryuna 0:dfe81bdcb486 9 *　
ryuna 0:dfe81bdcb486 10 *　servoにステアリング指示
ryuna 0:dfe81bdcb486 11 *　
ryuna 0:dfe81bdcb486 12 *　LCDでデバック
ryuna 0:dfe81bdcb486 13 *
ryuna 0:dfe81bdcb486 14 *　スイッチ4つとスタートスイッチで処理を実行
ryuna 0:dfe81bdcb486 15 *
ryuna 0:dfe81bdcb486 16
ryuna 0:dfe81bdcb486 17 *************************
ryuna 0:dfe81bdcb486 18 * Pin Map
ryuna 0:dfe81bdcb486 19 *
ryuna 0:dfe81bdcb486 20 * p5~p8 >> InterruptIn >> LineSensor
ryuna 0:dfe81bdcb486 21 *
ryuna 0:dfe81bdcb486 22 * p9,p10 >> I2C >> LPC1114FN28/102 read
ryuna 0:dfe81bdcb486 23 *
ryuna 0:dfe81bdcb486 24 * p13,p14 >> Serial >> Motor
ryuna 0:dfe81bdcb486 25 *
ryuna 0:dfe81bdcb486 26 * p21 >> PwmOut >> Servo
ryuna 0:dfe81bdcb486 27 *
ryuna 0:dfe81bdcb486 28 * p22~p26 >> DigitalIn >> DebugSw and StartSw
ryuna 0:dfe81bdcb486 29 *
ryuna 0:dfe81bdcb486 30 * p27,p28 >> I2C >> DebugLCD
ryuna 0:dfe81bdcb486 31 *
ryuna 0:dfe81bdcb486 32 * p29 >> DigitalOut >> Kicker
ryuna 0:dfe81bdcb486 33 *
ryuna 0:dfe81bdcb486 34 * **now , never use pin number p11,p12,p15,p16,p17,p18,p19,p20, p29,p30
ryuna 0:dfe81bdcb486 35 *
ryuna 0:dfe81bdcb486 36 * will use kicker, any Pin set in DigitalOut soon.
ryuna 0:dfe81bdcb486 37 *
ryuna 0:dfe81bdcb486 38 *
ryuna 0:dfe81bdcb486 39 ******************************/
ryuna 0:dfe81bdcb486 40
ryuna 0:dfe81bdcb486 41
ryuna 0:dfe81bdcb486 42
ryuna 0:dfe81bdcb486 43
ryuna 0:dfe81bdcb486 44
ryuna 0:dfe81bdcb486 45
ryuna 0:dfe81bdcb486 46 #include "mbed.h"
ryuna 0:dfe81bdcb486 47 #include "PID.h"
ryuna 0:dfe81bdcb486 48 #include "AQM0802A.h"
ryuna 0:dfe81bdcb486 49 #include "HMC6352.h"
ryuna 0:dfe81bdcb486 50 #include "Servo.h"
ryuna 0:dfe81bdcb486 51 #include "main.h"
ryuna 0:dfe81bdcb486 52 #include <math.h>
ryuna 0:dfe81bdcb486 53 #include <sstream>
ryuna 0:dfe81bdcb486 54
ryuna 0:dfe81bdcb486 55 /*回り込みの計算用*/
ryuna 0:dfe81bdcb486 56 #define PI 3.1415926
ryuna 0:dfe81bdcb486 57
ryuna 0:dfe81bdcb486 58 /*LPC1114 I2C Slave Address*/
ryuna 0:dfe81bdcb486 59 #define ADDRESS_R 0xAA
ryuna 0:dfe81bdcb486 60 #define ADDRESS_L 0xC0
ryuna 0:dfe81bdcb486 61
ryuna 0:dfe81bdcb486 62 /*BusIn sw　入力値*/
ryuna 0:dfe81bdcb486 63 #define Calibration 0x01
ryuna 0:dfe81bdcb486 64 #define Kicker 0x02
ryuna 0:dfe81bdcb486 65 #define Debug1 0x04
ryuna 0:dfe81bdcb486 66 #define Debug2 0x08
ryuna 0:dfe81bdcb486 67 #define StartS 0x10
ryuna 0:dfe81bdcb486 68
ryuna 0:dfe81bdcb486 69 /*send to LPC1114 ,receive data choice*/
ryuna 0:dfe81bdcb486 70 #define READ_IR 0x01
ryuna 0:dfe81bdcb486 71 #define READ_PING 0x02
ryuna 0:dfe81bdcb486 72
ryuna 0:dfe81bdcb486 73
ryuna 0:dfe81bdcb486 74
ryuna 0:dfe81bdcb486 75
ryuna 0:dfe81bdcb486 76 BusIn Sw(p22,p23,p24,p25,p26);
ryuna 0:dfe81bdcb486 77 BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
ryuna 0:dfe81bdcb486 78 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 0:dfe81bdcb486 79 DigitalOut Kick(p29);
ryuna 0:dfe81bdcb486 80
ryuna 0:dfe81bdcb486 81 Servo S555(p21);
ryuna 0:dfe81bdcb486 82 I2C Sensor(p9,p10);//sda,scl
ryuna 0:dfe81bdcb486 83 HMC6352 hmc6352(p9,p10);//sda,scl
ryuna 0:dfe81bdcb486 84 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 0:dfe81bdcb486 85 Serial Motor(p13,p14);//tx,rx
ryuna 0:dfe81bdcb486 86 Serial pc(USBTX,USBRX);
ryuna 0:dfe81bdcb486 87
ryuna 0:dfe81bdcb486 88 extern string StringFIN;
ryuna 0:dfe81bdcb486 89 extern void array(int,int,int,int);
ryuna 0:dfe81bdcb486 90
ryuna 0:dfe81bdcb486 91 int speed[4] = {0};
ryuna 0:dfe81bdcb486 92
ryuna 0:dfe81bdcb486 93 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
ryuna 0:dfe81bdcb486 94
ryuna 0:dfe81bdcb486 95
ryuna 0:dfe81bdcb486 96
ryuna 0:dfe81bdcb486 97 uint8_t IrReceive(uint8_t IrData[]){
ryuna 0:dfe81bdcb486 98 /*
ryuna 0:dfe81bdcb486 99 *Irdata[0] : 1位,2位の場所(1位*13+2位)
ryuna 0:dfe81bdcb486 100 *Irdata[1] : 1位の値
ryuna 0:dfe81bdcb486 101 *return : 1位の場所
ryuna 0:dfe81bdcb486 102 */
ryuna 0:dfe81bdcb486 103 char data_r[3],data_l[3];
ryuna 0:dfe81bdcb486 104 bool val;
ryuna 0:dfe81bdcb486 105 Sensor.write(READ_IR);//一斉送信のつもり
ryuna 0:dfe81bdcb486 106
ryuna 0:dfe81bdcb486 107 val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
ryuna 0:dfe81bdcb486 108 Led[0] = val;
ryuna 0:dfe81bdcb486 109 wait_ms(5);
ryuna 0:dfe81bdcb486 110 val = Sensor.read(ADDRESS_L|1, data_l, 3);
ryuna 0:dfe81bdcb486 111 Led[0] = !val;
ryuna 0:dfe81bdcb486 112
ryuna 0:dfe81bdcb486 113 if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
ryuna 0:dfe81bdcb486 114 if((data_r[0] == 0)&&(data_l[0] == 0)){
ryuna 0:dfe81bdcb486 115 IrData[0] = 12*13 + 12;
ryuna 0:dfe81bdcb486 116 return 12;
ryuna 0:dfe81bdcb486 117 }
ryuna 0:dfe81bdcb486 118 if(data_r[0] == 0){
ryuna 0:dfe81bdcb486 119 IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6);
ryuna 0:dfe81bdcb486 120 IrData[1] = data_l[1];
ryuna 0:dfe81bdcb486 121 return data_l[0]/6+6;
ryuna 0:dfe81bdcb486 122
ryuna 0:dfe81bdcb486 123 }
ryuna 0:dfe81bdcb486 124
ryuna 0:dfe81bdcb486 125 IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6);
ryuna 0:dfe81bdcb486 126 IrData[1] = data_r[1];
ryuna 0:dfe81bdcb486 127 return data_r[0]/6;
ryuna 0:dfe81bdcb486 128 }
ryuna 0:dfe81bdcb486 129
ryuna 0:dfe81bdcb486 130 if(data_r[2]<data_l[2]){
ryuna 0:dfe81bdcb486 131 if(data_r[2]<data_l[1]){
ryuna 0:dfe81bdcb486 132 IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6);
ryuna 0:dfe81bdcb486 133 IrData[1] = data_r[1];
ryuna 0:dfe81bdcb486 134 return data_r[0]/6;
ryuna 0:dfe81bdcb486 135 }
ryuna 0:dfe81bdcb486 136
ryuna 0:dfe81bdcb486 137 IrData[0] = (data_r[0]/6)*13 + (data_l[0]/6+6);
ryuna 0:dfe81bdcb486 138 IrData[1] = data_r[1];
ryuna 0:dfe81bdcb486 139 return data_r[0]/6;
ryuna 0:dfe81bdcb486 140
ryuna 0:dfe81bdcb486 141 }else{
ryuna 0:dfe81bdcb486 142 if(data_l[2]<data_r[1]){
ryuna 0:dfe81bdcb486 143 IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6);
ryuna 0:dfe81bdcb486 144 IrData[1] = data_l[1];
ryuna 0:dfe81bdcb486 145 return (data_l[0]/6+6);
ryuna 0:dfe81bdcb486 146 }
ryuna 0:dfe81bdcb486 147
ryuna 0:dfe81bdcb486 148 IrData[0] = (data_l[0]/6+6)*13 + (data_r[0]/6);
ryuna 0:dfe81bdcb486 149 IrData[1] = data_l[1];
ryuna 0:dfe81bdcb486 150 return (data_l[0]/6+6);
ryuna 0:dfe81bdcb486 151 }
ryuna 0:dfe81bdcb486 152
ryuna 0:dfe81bdcb486 153
ryuna 0:dfe81bdcb486 154 }
ryuna 0:dfe81bdcb486 155
ryuna 0:dfe81bdcb486 156 void PingReceiveRL(char ping[]){
ryuna 0:dfe81bdcb486 157 /*
ryuna 0:dfe81bdcb486 158 * ping[0] : Right ping data
ryuna 0:dfe81bdcb486 159 * ping[1] : Left ping data
ryuna 0:dfe81bdcb486 160 */
ryuna 0:dfe81bdcb486 161 char ReadSelect[1] = {READ_PING};
ryuna 0:dfe81bdcb486 162 bool val;
ryuna 0:dfe81bdcb486 163 val = Sensor.write(ADDRESS_R|0, ReadSelect , 1);
ryuna 0:dfe81bdcb486 164 Led[2] = val;
ryuna 0:dfe81bdcb486 165 val = Sensor.read(ADDRESS_R|1, ping, 2);
ryuna 0:dfe81bdcb486 166 Led[2] = !val;
ryuna 0:dfe81bdcb486 167
ryuna 0:dfe81bdcb486 168 }
ryuna 0:dfe81bdcb486 169
ryuna 0:dfe81bdcb486 170 void PingReceiveFB(char ping[]){
ryuna 0:dfe81bdcb486 171 /*
ryuna 0:dfe81bdcb486 172 * ping[0] : FRONT ping data
ryuna 0:dfe81bdcb486 173 * ping[1] : BACK ping data
ryuna 0:dfe81bdcb486 174 */
ryuna 0:dfe81bdcb486 175 char ReadSelect[1] = {READ_PING};
ryuna 0:dfe81bdcb486 176 bool val;
ryuna 0:dfe81bdcb486 177 val = Sensor.write(ADDRESS_L|0, ReadSelect , 1);
ryuna 0:dfe81bdcb486 178 Led[2] = val;
ryuna 0:dfe81bdcb486 179 val = Sensor.read(ADDRESS_L|1, ping, 2);
ryuna 0:dfe81bdcb486 180 Led[2] = !val;
ryuna 0:dfe81bdcb486 181
ryuna 0:dfe81bdcb486 182
ryuna 0:dfe81bdcb486 183 }
ryuna 0:dfe81bdcb486 184
ryuna 0:dfe81bdcb486 185
ryuna 0:dfe81bdcb486 186
ryuna 0:dfe81bdcb486 187 void move(int vr, int vl){
ryuna 0:dfe81bdcb486 188 /**
ryuna 0:dfe81bdcb486 189 * pwm[1] : right wheel(+data = forward/ -data = backward)
ryuna 0:dfe81bdcb486 190 * pwm[2] : left wheel(+data = forward/ -data = backward)
ryuna 0:dfe81bdcb486 191 * pwm[3] : not used
ryuna 0:dfe81bdcb486 192 * pwm[4] : not used
ryuna 0:dfe81bdcb486 193 *
ryuna 0:dfe81bdcb486 194 */
ryuna 0:dfe81bdcb486 195
ryuna 0:dfe81bdcb486 196 double pwm[4] = {0};
ryuna 0:dfe81bdcb486 197 uint8_t i = 0;
ryuna 0:dfe81bdcb486 198 pwm[0] = -vr;
ryuna 0:dfe81bdcb486 199 pwm[1] = 0;
ryuna 0:dfe81bdcb486 200 pwm[2] = 0;
ryuna 0:dfe81bdcb486 201 pwm[3] = vl;
ryuna 0:dfe81bdcb486 202
ryuna 0:dfe81bdcb486 203 for(i = 0; i < 4; i++){
ryuna 0:dfe81bdcb486 204 if(pwm[i] > 100){
ryuna 0:dfe81bdcb486 205 pwm[i] = 100;
ryuna 0:dfe81bdcb486 206 }else if(pwm[i] < -100){
ryuna 0:dfe81bdcb486 207 pwm[i] = -100;
ryuna 0:dfe81bdcb486 208 }
ryuna 0:dfe81bdcb486 209 speed[i] = pwm[i];
ryuna 0:dfe81bdcb486 210 }
ryuna 0:dfe81bdcb486 211 }
ryuna 0:dfe81bdcb486 212
ryuna 0:dfe81bdcb486 213
ryuna 0:dfe81bdcb486 214 void IrFrontAction(int *CompassDef , uint8_t IrMemo[])//ball 12 or 0 o-clock
ryuna 0:dfe81bdcb486 215 {
ryuna 0:dfe81bdcb486 216 /*前にボールがある場合の動作*/
ryuna 0:dfe81bdcb486 217 int Compass;
ryuna 0:dfe81bdcb486 218 char Ping[2];
ryuna 0:dfe81bdcb486 219 uint8_t Line,LineIr;
ryuna 0:dfe81bdcb486 220 unsigned int IrNumMax;
ryuna 0:dfe81bdcb486 221
ryuna 0:dfe81bdcb486 222
ryuna 0:dfe81bdcb486 223 if(IrReceive(IrMemo)<=150){//適当な150
ryuna 0:dfe81bdcb486 224 /*ステッピングを正面に設定*/
ryuna 0:dfe81bdcb486 225 /*busy_wait()*/
ryuna 0:dfe81bdcb486 226 /*モーターを前進*/
ryuna 0:dfe81bdcb486 227 return;
ryuna 0:dfe81bdcb486 228 }
ryuna 0:dfe81bdcb486 229 PingReceiveRL(Ping);
ryuna 0:dfe81bdcb486 230 if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当
ryuna 0:dfe81bdcb486 231 /*ステッピングを少しだけ傾ける*/
ryuna 0:dfe81bdcb486 232 /*モーターを左右差つけて回す(ロボットは少し傾く)*/
ryuna 0:dfe81bdcb486 233 /*busy_wait()*/
ryuna 0:dfe81bdcb486 234 /*モーターを前進*/
ryuna 0:dfe81bdcb486 235 while(1){
ryuna 0:dfe81bdcb486 236
ryuna 0:dfe81bdcb486 237 if(Line){ //ラインを検知していないか
ryuna 0:dfe81bdcb486 238 //LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
ryuna 0:dfe81bdcb486 239 while(LineIr){
ryuna 0:dfe81bdcb486 240 move(0,0);//
ryuna 0:dfe81bdcb486 241 Led[1] = Led[2] = Led[3] = 1;
ryuna 0:dfe81bdcb486 242
ryuna 0:dfe81bdcb486 243 //LineIr = LineReceive() & (IrReceiveS()/3 + 1);
ryuna 0:dfe81bdcb486 244 }
ryuna 0:dfe81bdcb486 245 Led[1] = Led[2] = Led[3] = 0;
ryuna 0:dfe81bdcb486 246
ryuna 0:dfe81bdcb486 247 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 0:dfe81bdcb486 248 while(!((Compass>=150)&&(Compass<=210))){
ryuna 0:dfe81bdcb486 249 move(20,-20);//適当な回転。
ryuna 0:dfe81bdcb486 250 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 0:dfe81bdcb486 251 }
ryuna 0:dfe81bdcb486 252 return;
ryuna 0:dfe81bdcb486 253 }
ryuna 0:dfe81bdcb486 254 Kick = 1;
ryuna 0:dfe81bdcb486 255 wait_ms(200);
ryuna 0:dfe81bdcb486 256 Kick = 0;
ryuna 0:dfe81bdcb486 257
ryuna 0:dfe81bdcb486 258 //IrNumMax = IrReceiveSM();//位置と大きさ
ryuna 0:dfe81bdcb486 259
ryuna 0:dfe81bdcb486 260 if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す.
ryuna 0:dfe81bdcb486 261 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 0:dfe81bdcb486 262 while(!((Compass>=150)&&(Compass<=210))){
ryuna 0:dfe81bdcb486 263 move(20,-20);//適当な回転。
ryuna 0:dfe81bdcb486 264 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 0:dfe81bdcb486 265 }
ryuna 0:dfe81bdcb486 266 return;
ryuna 0:dfe81bdcb486 267 }
ryuna 0:dfe81bdcb486 268
ryuna 0:dfe81bdcb486 269 if((IrNumMax&0x00FF)<150){
ryuna 0:dfe81bdcb486 270 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 0:dfe81bdcb486 271 while(!((Compass>=150)&&(Compass<=210))){
ryuna 0:dfe81bdcb486 272 move(20,-20);//適当な回転。
ryuna 0:dfe81bdcb486 273 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 0:dfe81bdcb486 274 }
ryuna 0:dfe81bdcb486 275 return;
ryuna 0:dfe81bdcb486 276 }
ryuna 0:dfe81bdcb486 277
ryuna 0:dfe81bdcb486 278 }
ryuna 0:dfe81bdcb486 279
ryuna 0:dfe81bdcb486 280 }else if((Ping[0]>40)&&(Ping[1]>90)){
ryuna 0:dfe81bdcb486 281
ryuna 0:dfe81bdcb486 282 }else{
ryuna 0:dfe81bdcb486 283
ryuna 0:dfe81bdcb486 284 }
ryuna 0:dfe81bdcb486 285
ryuna 0:dfe81bdcb486 286 }
ryuna 0:dfe81bdcb486 287
ryuna 0:dfe81bdcb486 288 void IrBackAction(int *CompassDef , uint8_t IrMemo[])//ball found six o-clock
ryuna 0:dfe81bdcb486 289 {
ryuna 0:dfe81bdcb486 290 /***
ryuna 0:dfe81bdcb486 291 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
ryuna 0:dfe81bdcb486 292 *
ryuna 0:dfe81bdcb486 293 **/
ryuna 0:dfe81bdcb486 294 char Ping[2];
ryuna 0:dfe81bdcb486 295
ryuna 0:dfe81bdcb486 296 PingReceiveRL(Ping);
ryuna 0:dfe81bdcb486 297
ryuna 0:dfe81bdcb486 298
ryuna 0:dfe81bdcb486 299 if(IrMemo[0] == 6){//check ir number
ryuna 0:dfe81bdcb486 300 return ;
ryuna 0:dfe81bdcb486 301 }
ryuna 0:dfe81bdcb486 302 if(Ping[0]<Ping[1]){
ryuna 0:dfe81bdcb486 303 /*右が大きい*/
ryuna 0:dfe81bdcb486 304
ryuna 0:dfe81bdcb486 305
ryuna 0:dfe81bdcb486 306 return;
ryuna 0:dfe81bdcb486 307 }
ryuna 0:dfe81bdcb486 308 /*左が大きい*/
ryuna 0:dfe81bdcb486 309
ryuna 0:dfe81bdcb486 310
ryuna 0:dfe81bdcb486 311
ryuna 0:dfe81bdcb486 312
ryuna 0:dfe81bdcb486 313 }
ryuna 0:dfe81bdcb486 314 void GoHome(int *CompassDef , uint8_t IrMemo[])//Ball is not found.
ryuna 0:dfe81bdcb486 315 {
ryuna 0:dfe81bdcb486 316 /*line検知をしないバージョン*/
ryuna 0:dfe81bdcb486 317 int Compass;
ryuna 0:dfe81bdcb486 318 char Ping[2] = {0};
ryuna 0:dfe81bdcb486 319
ryuna 0:dfe81bdcb486 320
ryuna 0:dfe81bdcb486 321 PingReceiveFB(Ping);
ryuna 0:dfe81bdcb486 322 if(Ping[1] >=60){//後ろからの距離60cm
ryuna 0:dfe81bdcb486 323 move(-20,-20);
ryuna 0:dfe81bdcb486 324 PingReceiveFB(Ping);
ryuna 0:dfe81bdcb486 325 return ;
ryuna 0:dfe81bdcb486 326 }
ryuna 0:dfe81bdcb486 327
ryuna 0:dfe81bdcb486 328 move(0,0);//stop
ryuna 0:dfe81bdcb486 329
ryuna 0:dfe81bdcb486 330 }
ryuna 0:dfe81bdcb486 331
ryuna 0:dfe81bdcb486 332
ryuna 0:dfe81bdcb486 333 uint8_t SwRead(){
ryuna 0:dfe81bdcb486 334 /******
ryuna 0:dfe81bdcb486 335 *retrun : sw_state
ryuna 0:dfe81bdcb486 336 *Calibration = 0x01;
ryuna 0:dfe81bdcb486 337 *Kicker = 0x02;
ryuna 0:dfe81bdcb486 338 *Debug1 = 0x04;
ryuna 0:dfe81bdcb486 339 *Debug2 = 0x08;
ryuna 0:dfe81bdcb486 340 *StartS = 0x10;
ryuna 0:dfe81bdcb486 341 *
ryuna 0:dfe81bdcb486 342 *****/
ryuna 0:dfe81bdcb486 343 uint8_t i,temp,temp2;
ryuna 0:dfe81bdcb486 344 temp = Sw;
ryuna 0:dfe81bdcb486 345 if(!(temp == Calibration
ryuna 0:dfe81bdcb486 346 ||temp == Kicker
ryuna 0:dfe81bdcb486 347 ||temp == Debug1
ryuna 0:dfe81bdcb486 348 ||temp == Debug2
ryuna 0:dfe81bdcb486 349 ||temp == StartS)) return 0;/*スイッチが押されていない*/
ryuna 0:dfe81bdcb486 350 if(temp !=0x00){
ryuna 0:dfe81bdcb486 351 for(i = 0; i < 50; i++);
ryuna 0:dfe81bdcb486 352 temp2 = Sw;
ryuna 0:dfe81bdcb486 353 if(temp == temp2){
ryuna 0:dfe81bdcb486 354 return temp;
ryuna 0:dfe81bdcb486 355 }
ryuna 0:dfe81bdcb486 356 }
ryuna 0:dfe81bdcb486 357 return 0;
ryuna 0:dfe81bdcb486 358 }
ryuna 0:dfe81bdcb486 359
ryuna 0:dfe81bdcb486 360 //通信（モータ用）
ryuna 0:dfe81bdcb486 361 void tx_motor(){
ryuna 0:dfe81bdcb486 362 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:dfe81bdcb486 363 Motor.printf("%s",StringFIN.c_str());
ryuna 0:dfe81bdcb486 364 }
ryuna 0:dfe81bdcb486 365
ryuna 0:dfe81bdcb486 366 void SetUp(int *compass_def){
ryuna 0:dfe81bdcb486 367 /*初期化*/
ryuna 0:dfe81bdcb486 368
ryuna 0:dfe81bdcb486 369 Kick = 0;
ryuna 0:dfe81bdcb486 370 Sw.mode(PullUp);
ryuna 0:dfe81bdcb486 371
ryuna 0:dfe81bdcb486 372 Motor.baud(115200); //ボーレート設定
ryuna 0:dfe81bdcb486 373 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:dfe81bdcb486 374 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:dfe81bdcb486 375 //move(0,0);//停止
ryuna 0:dfe81bdcb486 376
ryuna 0:dfe81bdcb486 377 S555.calibrate(0.0005, 120.0);
ryuna 0:dfe81bdcb486 378 S555.position(0.0); //初期位置にセット
ryuna 0:dfe81bdcb486 379 /*
ryuna 0:dfe81bdcb486 380 hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ryuna 0:dfe81bdcb486 381 *compass_def = (hmc6352.sample() / 10);
ryuna 0:dfe81bdcb486 382 */
ryuna 0:dfe81bdcb486 383 //Lcd.cls();
ryuna 0:dfe81bdcb486 384
ryuna 0:dfe81bdcb486 385 }
ryuna 0:dfe81bdcb486 386 void StartLoop(){
ryuna 0:dfe81bdcb486 387 /*
ryuna 0:dfe81bdcb486 388 *スイッチが押されるまでロボットはスタートしない．
ryuna 0:dfe81bdcb486 389 *
ryuna 0:dfe81bdcb486 390 *switch割り当て
ryuna 0:dfe81bdcb486 391 *1.コンパスのキャリブレーション実行スイッチ
ryuna 0:dfe81bdcb486 392 *2.キッカーのキック(check用)
ryuna 0:dfe81bdcb486 393 *3,4.自由
ryuna 0:dfe81bdcb486 394 *5.StartSw
ryuna 0:dfe81bdcb486 395 */
ryuna 0:dfe81bdcb486 396 uint8_t State = 0;
ryuna 0:dfe81bdcb486 397
ryuna 0:dfe81bdcb486 398 while(1){
ryuna 0:dfe81bdcb486 399
ryuna 0:dfe81bdcb486 400 State = SwRead();
ryuna 0:dfe81bdcb486 401 if(State == 0) continue;
ryuna 0:dfe81bdcb486 402
ryuna 0:dfe81bdcb486 403 if(State == Calibration){
ryuna 0:dfe81bdcb486 404 /*calibration command enter...
ryuna 0:dfe81bdcb486 405 *
ryuna 0:dfe81bdcb486 406 *
ryuna 0:dfe81bdcb486 407 */
ryuna 0:dfe81bdcb486 408 continue;
ryuna 0:dfe81bdcb486 409 }
ryuna 0:dfe81bdcb486 410 if(State == Kicker){
ryuna 0:dfe81bdcb486 411 /*
ryuna 0:dfe81bdcb486 412 *kicker check
ryuna 0:dfe81bdcb486 413 *
ryuna 0:dfe81bdcb486 414 *
ryuna 0:dfe81bdcb486 415 */
ryuna 0:dfe81bdcb486 416 continue;
ryuna 0:dfe81bdcb486 417 }
ryuna 0:dfe81bdcb486 418 if(State == Debug1){
ryuna 0:dfe81bdcb486 419 /* debug command free
ryuna 0:dfe81bdcb486 420 *
ryuna 0:dfe81bdcb486 421 *display out to selected 3 menus. compass, ir, ping, line,etc...
ryuna 0:dfe81bdcb486 422 *
ryuna 0:dfe81bdcb486 423 **/
ryuna 0:dfe81bdcb486 424
ryuna 0:dfe81bdcb486 425 }
ryuna 0:dfe81bdcb486 426 if(State == Debug2){
ryuna 0:dfe81bdcb486 427 /* debug command free
ryuna 0:dfe81bdcb486 428 *
ryuna 0:dfe81bdcb486 429 * decide movement of the beginning.
ryuna 0:dfe81bdcb486 430 *
ryuna 0:dfe81bdcb486 431 *
ryuna 0:dfe81bdcb486 432 **/
ryuna 0:dfe81bdcb486 433
ryuna 0:dfe81bdcb486 434 }
ryuna 0:dfe81bdcb486 435 if(State == StartS){
ryuna 0:dfe81bdcb486 436 /*game start...*/
ryuna 0:dfe81bdcb486 437 return;
ryuna 0:dfe81bdcb486 438 }
ryuna 0:dfe81bdcb486 439
ryuna 0:dfe81bdcb486 440 }
ryuna 0:dfe81bdcb486 441
ryuna 0:dfe81bdcb486 442 }
ryuna 0:dfe81bdcb486 443 int main() {
ryuna 0:dfe81bdcb486 444
ryuna 0:dfe81bdcb486 445 /*Ir*/
ryuna 0:dfe81bdcb486 446 uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
ryuna 0:dfe81bdcb486 447 uint8_t IrData[2];//0:1位*13+2位,1:1位の値
ryuna 0:dfe81bdcb486 448 /*Line*/
ryuna 0:dfe81bdcb486 449 uint8_t LineData = 0;
ryuna 0:dfe81bdcb486 450
ryuna 0:dfe81bdcb486 451 /*Compass*/
ryuna 0:dfe81bdcb486 452 int CompassDef = 0, Compass = 0;
ryuna 0:dfe81bdcb486 453
ryuna 0:dfe81bdcb486 454 /*State */
ryuna 0:dfe81bdcb486 455 //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
ryuna 0:dfe81bdcb486 456 uint8_t LineIr = NA;
ryuna 0:dfe81bdcb486 457 uint8_t IrChange[13] ={1,1,1,2,2,2,4,4,4,8,8,8};
ryuna 0:dfe81bdcb486 458
ryuna 0:dfe81bdcb486 459 /*関数ポインタ*/
ryuna 0:dfe81bdcb486 460 void (*AnotherAction[3])(int *, uint8_t []);//irの位置による分岐
ryuna 0:dfe81bdcb486 461 AnotherAction[0] = IrFrontAction;
ryuna 0:dfe81bdcb486 462 AnotherAction[1] = IrBackAction;
ryuna 0:dfe81bdcb486 463 AnotherAction[2] = GoHome;
ryuna 0:dfe81bdcb486 464
ryuna 0:dfe81bdcb486 465
ryuna 0:dfe81bdcb486 466 SetUp(&CompassDef);/*set up routine*/
ryuna 0:dfe81bdcb486 467 //StartLoop(); /*loop stat, switch push end*/
ryuna 0:dfe81bdcb486 468
ryuna 0:dfe81bdcb486 469 int val = 0;
ryuna 0:dfe81bdcb486 470 char data[3] = {0};
ryuna 0:dfe81bdcb486 471 char order[1] = {0};
ryuna 0:dfe81bdcb486 472 while(1) {
ryuna 0:dfe81bdcb486 473
ryuna 0:dfe81bdcb486 474
ryuna 0:dfe81bdcb486 475
ryuna 0:dfe81bdcb486 476 /*白線を読んでいないか確認する*/
ryuna 0:dfe81bdcb486 477 /*
ryuna 0:dfe81bdcb486 478 LineData = (~Line+0x00) & 0x0F;
ryuna 0:dfe81bdcb486 479 if(LineData){
ryuna 0:dfe81bdcb486 480 IrNum = IrReceive(IrData);
ryuna 0:dfe81bdcb486 481 LineIr = LineData & IrChange[IrNum]; //一箇所でも一致すればlineを検知している．
ryuna 0:dfe81bdcb486 482 while(LineIr){
ryuna 0:dfe81bdcb486 483 move(0,0);//
ryuna 0:dfe81bdcb486 484 Led[1] = Led[2] = Led[3] = 1;
ryuna 0:dfe81bdcb486 485 LineData = (~Line+0x00) & 0x0F;
ryuna 0:dfe81bdcb486 486 IrNum = IrReceive(IrData);
ryuna 0:dfe81bdcb486 487 LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している．
ryuna 0:dfe81bdcb486 488 }
ryuna 0:dfe81bdcb486 489 Led[1] = Led[2] = Led[3] = 0;
ryuna 0:dfe81bdcb486 490 continue;
ryuna 0:dfe81bdcb486 491 }
ryuna 0:dfe81bdcb486 492 */
ryuna 0:dfe81bdcb486 493
ryuna 0:dfe81bdcb486 494
ryuna 0:dfe81bdcb486 495
ryuna 0:dfe81bdcb486 496 /*ロボットの向いている向きが正面か確認する*/
ryuna 0:dfe81bdcb486 497 /*
ryuna 0:dfe81bdcb486 498 Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
ryuna 0:dfe81bdcb486 499 if(!((Compass>=150)&&(Compass<=210))){
ryuna 0:dfe81bdcb486 500 while(!((Compass>=150)&&(Compass<=210))){
ryuna 0:dfe81bdcb486 501 move(20,-20);
ryuna 0:dfe81bdcb486 502 Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
ryuna 0:dfe81bdcb486 503 }
ryuna 0:dfe81bdcb486 504 continue;
ryuna 0:dfe81bdcb486 505
ryuna 0:dfe81bdcb486 506 }
ryuna 0:dfe81bdcb486 507 */
ryuna 0:dfe81bdcb486 508 /*ボールの位置を知る*/
ryuna 0:dfe81bdcb486 509 //IrNum = IrReceive(IrData);
ryuna 0:dfe81bdcb486 510 val = Sensor.write(ADDRESS_R|0, order , 1);
ryuna 0:dfe81bdcb486 511
ryuna 0:dfe81bdcb486 512 Led[1] = !val;
ryuna 0:dfe81bdcb486 513
ryuna 0:dfe81bdcb486 514 wait_ms(5);
ryuna 0:dfe81bdcb486 515
ryuna 0:dfe81bdcb486 516 val = Sensor.read(ADDRESS_R|1, data, 3);// IRデータを受信
ryuna 0:dfe81bdcb486 517 Led[0] = !val;
ryuna 0:dfe81bdcb486 518 pc.printf("num = %d irData1 = %d irData2 = %d\n",(int)data[0],(int)data[1],(int)data[2]);
ryuna 0:dfe81bdcb486 519
ryuna 0:dfe81bdcb486 520 wait_ms(0.1);
ryuna 0:dfe81bdcb486 521
ryuna 0:dfe81bdcb486 522 /*複雑な処理が必要な箇所については関数に飛ばす*/
ryuna 0:dfe81bdcb486 523 /*
ryuna 0:dfe81bdcb486 524
ryuna 0:dfe81bdcb486 525 if(IrNum == 0 || IrNum == 6||IrNum == 12){
ryuna 0:dfe81bdcb486 526 (*AnotherAction[IrNum/6])(&CompassDef,IrData); //Front OR Back Action
ryuna 0:dfe81bdcb486 527 continue;
ryuna 0:dfe81bdcb486 528 }
ryuna 0:dfe81bdcb486 529
ryuna 0:dfe81bdcb486 530 */
ryuna 0:dfe81bdcb486 531
ryuna 0:dfe81bdcb486 532 /*残りの箇所についての処理を行う*/
ryuna 0:dfe81bdcb486 533
ryuna 0:dfe81bdcb486 534
ryuna 0:dfe81bdcb486 535
ryuna 0:dfe81bdcb486 536 //wait_ms(0);
ryuna 0:dfe81bdcb486 537
ryuna 0:dfe81bdcb486 538
ryuna 0:dfe81bdcb486 539 }
ryuna 0:dfe81bdcb486 540
ryuna 0:dfe81bdcb486 541
ryuna 0:dfe81bdcb486 542 while(0){
ryuna 0:dfe81bdcb486 543 /*デモプログラム*/
ryuna 0:dfe81bdcb486 544
ryuna 0:dfe81bdcb486 545
ryuna 0:dfe81bdcb486 546 }
ryuna 0:dfe81bdcb486 547
ryuna 0:dfe81bdcb486 548
ryuna 0:dfe81bdcb486 549
ryuna 0:dfe81bdcb486 550
ryuna 0:dfe81bdcb486 551 }