CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

version 3 通信方式，マイコン等に変更あり

Dependencies:   AQM0802A PID Servo mbed

main.cpp@1:f91d53098d57, 2015-03-06 (annotated)

Committer:
ryuna
Date:
Fri Mar 06 02:15:10 2015 +0000
Revision:
1:f91d53098d57
Parent:
0:65b9e62cc2b6
Child:
2:e84bb87eea71
bug fix;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:65b9e62cc2b6 1 /***********************************
ryuna 0:65b9e62cc2b6 2 *RoboCupJunior Soccer B Open 2015
ryuna 0:65b9e62cc2b6 3 *Koshinnestu progrum
ryuna 0:65b9e62cc2b6 4 *
ryuna 0:65b9e62cc2b6 5 *　
ryuna 0:65b9e62cc2b6 6 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
ryuna 0:65b9e62cc2b6 7 *
ryuna 0:65b9e62cc2b6 8 *　MotorDriverにmaxonに命令
ryuna 0:65b9e62cc2b6 9 *　
ryuna 0:65b9e62cc2b6 10 *　servoにステアリング指示
ryuna 0:65b9e62cc2b6 11 *　
ryuna 0:65b9e62cc2b6 12 *　LCDでデバック
ryuna 0:65b9e62cc2b6 13 *
ryuna 0:65b9e62cc2b6 14 *　スイッチ4つとスタートスイッチで処理を実行
ryuna 0:65b9e62cc2b6 15 *
ryuna 0:65b9e62cc2b6 16
ryuna 0:65b9e62cc2b6 17 *************************
ryuna 0:65b9e62cc2b6 18 * Pin Map
ryuna 0:65b9e62cc2b6 19 *
ryuna 0:65b9e62cc2b6 20 * p5~p8 >> BusIn >> LineSensor
ryuna 0:65b9e62cc2b6 21 *
ryuna 0:65b9e62cc2b6 22 * p9,p10 >> I2C >> LPC1114FN28/102 read & Compass
ryuna 0:65b9e62cc2b6 23 *
ryuna 0:65b9e62cc2b6 24 * p13,p14 >> Serial >> Motor
ryuna 0:65b9e62cc2b6 25 *
ryuna 0:65b9e62cc2b6 26 * p21 >> PwmOut >> Servo
ryuna 0:65b9e62cc2b6 27 *
ryuna 0:65b9e62cc2b6 28 * p22~p26 >> DigitalIn >> DebugSw and StartSw
ryuna 0:65b9e62cc2b6 29 *
ryuna 0:65b9e62cc2b6 30 * p27,p28 >> I2C >> DebugLCD
ryuna 0:65b9e62cc2b6 31 *
ryuna 0:65b9e62cc2b6 32 * p29 >> DigitalOut >> Kicker
ryuna 0:65b9e62cc2b6 33 *
ryuna 0:65b9e62cc2b6 34 * *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30
ryuna 0:65b9e62cc2b6 35 *
ryuna 0:65b9e62cc2b6 36 *
ryuna 0:65b9e62cc2b6 37 ******************************/
ryuna 0:65b9e62cc2b6 38
ryuna 0:65b9e62cc2b6 39 #include "mbed.h"
ryuna 0:65b9e62cc2b6 40 #include <math.h>
ryuna 0:65b9e62cc2b6 41 #include <sstream>
ryuna 0:65b9e62cc2b6 42 #include "PID.h"
ryuna 0:65b9e62cc2b6 43 #include "AQM0802A.h"
ryuna 0:65b9e62cc2b6 44 #include "MultiSerial.h"
ryuna 0:65b9e62cc2b6 45 #include "Servo.h"
ryuna 0:65b9e62cc2b6 46 #include "main.h"
ryuna 0:65b9e62cc2b6 47
ryuna 0:65b9e62cc2b6 48
ryuna 0:65b9e62cc2b6 49 void Receive(){
ryuna 0:65b9e62cc2b6 50 IrData[0] = rx_data[0];
ryuna 0:65b9e62cc2b6 51 IrData[1] = rx_data[1];
ryuna 0:65b9e62cc2b6 52 IrData[2] = rx_data[2];
ryuna 0:65b9e62cc2b6 53 PingData[0] = rx_data[3];
ryuna 0:65b9e62cc2b6 54 PingData[1] = rx_data[4];
ryuna 0:65b9e62cc2b6 55 PingData[2] = rx_data[5];
ryuna 0:65b9e62cc2b6 56 PingData[3] = rx_data[6];
ryuna 0:65b9e62cc2b6 57 Compass = rx_data[7]+rx_data[8];
ryuna 0:65b9e62cc2b6 58
ryuna 0:65b9e62cc2b6 59 if(IrData[0] == 255){
ryuna 0:65b9e62cc2b6 60 IrNum = 12;
ryuna 0:65b9e62cc2b6 61 return;
ryuna 0:65b9e62cc2b6 62 }
ryuna 0:65b9e62cc2b6 63 IrNum = IrData[0]/12;
ryuna 0:65b9e62cc2b6 64
ryuna 0:65b9e62cc2b6 65
ryuna 0:65b9e62cc2b6 66 }
ryuna 0:65b9e62cc2b6 67
ryuna 0:65b9e62cc2b6 68 void move(int vr,int vl, double vs ,double Rad){
ryuna 0:65b9e62cc2b6 69 double pwm[4] = {0};
ryuna 0:65b9e62cc2b6 70 uint8_t i = 0;
ryuna 0:65b9e62cc2b6 71 pwm[0] = vr - vs;
ryuna 0:65b9e62cc2b6 72 pwm[1] = 0;
ryuna 0:65b9e62cc2b6 73 pwm[2] = 0;
ryuna 0:65b9e62cc2b6 74 pwm[3] = vl*1.2 + vs;
ryuna 0:65b9e62cc2b6 75
ryuna 0:65b9e62cc2b6 76 for(i = 0; i < 4; i++){
ryuna 0:65b9e62cc2b6 77 if(pwm[i] > 100){
ryuna 0:65b9e62cc2b6 78 pwm[i] = 100;
ryuna 0:65b9e62cc2b6 79 }else if(pwm[i] < -100){
ryuna 0:65b9e62cc2b6 80 pwm[i] = -100;
ryuna 0:65b9e62cc2b6 81 }
ryuna 0:65b9e62cc2b6 82 speed[i] = pwm[i];
ryuna 0:65b9e62cc2b6 83 }
ryuna 0:65b9e62cc2b6 84 SetRad = Rad;
ryuna 0:65b9e62cc2b6 85 wait_ms(10);
ryuna 0:65b9e62cc2b6 86 }
ryuna 0:65b9e62cc2b6 87 void fool (int *Rad, int *Power){
ryuna 0:65b9e62cc2b6 88 static int Last_Rad = 0;
ryuna 0:65b9e62cc2b6 89 static int Last_Vector = 1;
ryuna 0:65b9e62cc2b6 90 int rad = *Rad;
ryuna 0:65b9e62cc2b6 91 int Temp;
ryuna 0:65b9e62cc2b6 92 Temp = Last_Rad % 180;
ryuna 0:65b9e62cc2b6 93
ryuna 0:65b9e62cc2b6 94 if((Temp>70) &&(Temp<110)){
ryuna 0:65b9e62cc2b6 95 Temp = *Rad % 180;
ryuna 0:65b9e62cc2b6 96 if((Temp>70) &&(Temp<110)){
ryuna 0:65b9e62cc2b6 97 Temp = abs(*Rad - Last_Rad);
ryuna 0:65b9e62cc2b6 98 if(Temp>160){
ryuna 0:65b9e62cc2b6 99 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
ryuna 0:65b9e62cc2b6 100 if(*Rad/180){
ryuna 0:65b9e62cc2b6 101 *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
ryuna 0:65b9e62cc2b6 102 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 103 }else{
ryuna 0:65b9e62cc2b6 104 *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
ryuna 0:65b9e62cc2b6 105 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 106 }
ryuna 0:65b9e62cc2b6 107 Last_Rad = rad;
ryuna 0:65b9e62cc2b6 108 return;
ryuna 0:65b9e62cc2b6 109 }else if((Last_Vector+2) == 1){
ryuna 0:65b9e62cc2b6 110 /*逆転のまま角度拡張*/
ryuna 0:65b9e62cc2b6 111 if(*Rad/180){
ryuna 0:65b9e62cc2b6 112 *Rad = -360 + *Rad ;
ryuna 0:65b9e62cc2b6 113 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 114 }else{
ryuna 0:65b9e62cc2b6 115 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 116 }
ryuna 0:65b9e62cc2b6 117 Last_Rad = rad;
ryuna 0:65b9e62cc2b6 118 return;
ryuna 0:65b9e62cc2b6 119
ryuna 0:65b9e62cc2b6 120 }else if((Last_Vector+2) == 3){
ryuna 0:65b9e62cc2b6 121 /*正転のまま*/
ryuna 0:65b9e62cc2b6 122 if(*Rad/180){
ryuna 0:65b9e62cc2b6 123 *Rad = -360 + *Rad ;
ryuna 0:65b9e62cc2b6 124 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 125 }else{
ryuna 0:65b9e62cc2b6 126 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 127 Last_Rad = rad;
ryuna 0:65b9e62cc2b6 128 }
ryuna 0:65b9e62cc2b6 129 Last_Rad = rad;
ryuna 0:65b9e62cc2b6 130 return;
ryuna 0:65b9e62cc2b6 131 }
ryuna 0:65b9e62cc2b6 132 }
ryuna 0:65b9e62cc2b6 133 }
ryuna 0:65b9e62cc2b6 134 /*通常動作*/
ryuna 0:65b9e62cc2b6 135 Last_Vector = RadToVector[(*Rad-1)/90];
ryuna 0:65b9e62cc2b6 136 *Rad = Angle[*Rad%180];
ryuna 0:65b9e62cc2b6 137 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 138 Last_Rad = rad;
ryuna 0:65b9e62cc2b6 139
ryuna 0:65b9e62cc2b6 140 }
ryuna 0:65b9e62cc2b6 141 int IrRad(){
ryuna 0:65b9e62cc2b6 142 /*irの1位の値，2位の場所からirの360へ持っていく*/
ryuna 0:65b9e62cc2b6 143
ryuna 0:65b9e62cc2b6 144 uint8_t IrF =IrData[0]/12,IrS = IrData[0]%12;
ryuna 0:65b9e62cc2b6 145 int irrad;
ryuna 0:65b9e62cc2b6 146 irrad = 359 - IrF*30;
ryuna 0:65b9e62cc2b6 147
ryuna 0:65b9e62cc2b6 148 if(IrS){
ryuna 0:65b9e62cc2b6 149 if(IrF == 0 ){
ryuna 0:65b9e62cc2b6 150 if(IrS == 11){
ryuna 0:65b9e62cc2b6 151 irrad = 15;
ryuna 0:65b9e62cc2b6 152
ryuna 0:65b9e62cc2b6 153 }else{
ryuna 0:65b9e62cc2b6 154 irrad = 345;
ryuna 0:65b9e62cc2b6 155 }
ryuna 0:65b9e62cc2b6 156
ryuna 0:65b9e62cc2b6 157 }
ryuna 0:65b9e62cc2b6 158 irrad = irrad - (IrF - IrS)*15;
ryuna 0:65b9e62cc2b6 159
ryuna 0:65b9e62cc2b6 160 }
ryuna 0:65b9e62cc2b6 161 return irrad;
ryuna 0:65b9e62cc2b6 162 }
ryuna 0:65b9e62cc2b6 163
ryuna 0:65b9e62cc2b6 164
ryuna 0:65b9e62cc2b6 165 void IrFrontAction( uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
ryuna 0:65b9e62cc2b6 166 {
ryuna 0:65b9e62cc2b6 167
ryuna 0:65b9e62cc2b6 168 }
ryuna 0:65b9e62cc2b6 169
ryuna 0:65b9e62cc2b6 170 void IrBackAction( uint8_t IrMemo[],double ReV)//ball found six o-clock
ryuna 0:65b9e62cc2b6 171 {
ryuna 0:65b9e62cc2b6 172 /***
ryuna 0:65b9e62cc2b6 173 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
ryuna 0:65b9e62cc2b6 174 *
ryuna 0:65b9e62cc2b6 175 **/
ryuna 0:65b9e62cc2b6 176 char Ping[2]={0};
ryuna 0:65b9e62cc2b6 177
ryuna 0:65b9e62cc2b6 178 //PingReceiveRL(Ping);
ryuna 0:65b9e62cc2b6 179
ryuna 0:65b9e62cc2b6 180
ryuna 0:65b9e62cc2b6 181 if(IrMemo[0] == 6){//check ir number
ryuna 0:65b9e62cc2b6 182 return ;
ryuna 0:65b9e62cc2b6 183 }
ryuna 0:65b9e62cc2b6 184 if(Ping[0]<Ping[1]){
ryuna 0:65b9e62cc2b6 185 /*右が大きい*/
ryuna 0:65b9e62cc2b6 186
ryuna 0:65b9e62cc2b6 187
ryuna 0:65b9e62cc2b6 188 return;
ryuna 0:65b9e62cc2b6 189 }
ryuna 0:65b9e62cc2b6 190 /*左が大きい*/
ryuna 0:65b9e62cc2b6 191
ryuna 0:65b9e62cc2b6 192
ryuna 0:65b9e62cc2b6 193
ryuna 0:65b9e62cc2b6 194 }
ryuna 0:65b9e62cc2b6 195
ryuna 0:65b9e62cc2b6 196 void GoHome( uint8_t IrMemo[],double ReV)//Ball is not found.
ryuna 0:65b9e62cc2b6 197 {
ryuna 0:65b9e62cc2b6 198 //止まっとく
ryuna 0:65b9e62cc2b6 199 S555.position(0.0);
ryuna 0:65b9e62cc2b6 200
ryuna 0:65b9e62cc2b6 201 move(0,0,ReV,0);
ryuna 0:65b9e62cc2b6 202
ryuna 0:65b9e62cc2b6 203 /*line検知をしないバージョン*/
ryuna 0:65b9e62cc2b6 204 /*
ryuna 0:65b9e62cc2b6 205 char Ping[2] = {0};
ryuna 0:65b9e62cc2b6 206
ryuna 0:65b9e62cc2b6 207
ryuna 0:65b9e62cc2b6 208 PingReceiveFB(Ping);
ryuna 0:65b9e62cc2b6 209 if(Ping[1] >=60){//後ろからの距離60cm
ryuna 0:65b9e62cc2b6 210 move(-20,-20,ReV);
ryuna 0:65b9e62cc2b6 211 PingReceiveFB(Ping);
ryuna 0:65b9e62cc2b6 212 return ;
ryuna 0:65b9e62cc2b6 213 }
ryuna 0:65b9e62cc2b6 214
ryuna 0:65b9e62cc2b6 215 move(0,0,ReV);//stop
ryuna 0:65b9e62cc2b6 216 */
ryuna 0:65b9e62cc2b6 217 }
ryuna 0:65b9e62cc2b6 218
ryuna 0:65b9e62cc2b6 219
ryuna 0:65b9e62cc2b6 220 void PidUpdate()
ryuna 0:65b9e62cc2b6 221 {
ryuna 0:65b9e62cc2b6 222
ryuna 0:65b9e62cc2b6 223 //Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
ryuna 0:65b9e62cc2b6 224 wait_ms(10);
ryuna 0:65b9e62cc2b6 225
ryuna 0:65b9e62cc2b6 226 //pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 0:65b9e62cc2b6 227 //InputPID = Compass;
ryuna 0:65b9e62cc2b6 228
ryuna 0:65b9e62cc2b6 229 //pid.setProcessValue(InputPID);
ryuna 0:65b9e62cc2b6 230 //CompassPID = -(pid.compute());
ryuna 0:65b9e62cc2b6 231 }
ryuna 0:65b9e62cc2b6 232
ryuna 0:65b9e62cc2b6 233
ryuna 0:65b9e62cc2b6 234
ryuna 0:65b9e62cc2b6 235
ryuna 0:65b9e62cc2b6 236
ryuna 0:65b9e62cc2b6 237
ryuna 0:65b9e62cc2b6 238
ryuna 0:65b9e62cc2b6 239 uint8_t SwRead(){
ryuna 0:65b9e62cc2b6 240 /******
ryuna 0:65b9e62cc2b6 241 *retrun : sw_state
ryuna 0:65b9e62cc2b6 242 *StartS = 0x01;
ryuna 0:65b9e62cc2b6 243 *Debug2 = 0x02;
ryuna 0:65b9e62cc2b6 244 *Debug1 = 0x04;
ryuna 0:65b9e62cc2b6 245 *Debug3 = 0x06;
ryuna 0:65b9e62cc2b6 246 *Kicker = 0x08;
ryuna 0:65b9e62cc2b6 247 *Calibration = 0x10;
ryuna 0:65b9e62cc2b6 248
ryuna 0:65b9e62cc2b6 249 *
ryuna 0:65b9e62cc2b6 250 *****/
ryuna 0:65b9e62cc2b6 251 uint8_t i,temp,temp2;
ryuna 0:65b9e62cc2b6 252 temp = ~Sw - 224;
ryuna 0:65b9e62cc2b6 253 if(!(temp == Kicker
ryuna 0:65b9e62cc2b6 254 ||temp == Debug1
ryuna 0:65b9e62cc2b6 255 ||temp == Debug2
ryuna 0:65b9e62cc2b6 256 ||temp == Debug3
ryuna 0:65b9e62cc2b6 257 ||temp == StartS)) return 0;/*スイッチが押されていない*/
ryuna 0:65b9e62cc2b6 258 if(!(temp == 0x00)){
ryuna 0:65b9e62cc2b6 259 for(i = 0; i < 50; i++);
ryuna 0:65b9e62cc2b6 260 temp2 = ~Sw - 224;
ryuna 0:65b9e62cc2b6 261 if(temp == temp2){
ryuna 0:65b9e62cc2b6 262 return temp;
ryuna 0:65b9e62cc2b6 263 }
ryuna 0:65b9e62cc2b6 264 }
ryuna 0:65b9e62cc2b6 265 return 0;
ryuna 0:65b9e62cc2b6 266 }
ryuna 0:65b9e62cc2b6 267
ryuna 0:65b9e62cc2b6 268 //通信（モータ用）
ryuna 0:65b9e62cc2b6 269 void tx_motor(){
ryuna 0:65b9e62cc2b6 270 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:65b9e62cc2b6 271 Motor.printf("%s",StringFIN.c_str());
ryuna 0:65b9e62cc2b6 272 S555.position(SetRad);
ryuna 0:65b9e62cc2b6 273 }
ryuna 0:65b9e62cc2b6 274
ryuna 0:65b9e62cc2b6 275 void SetUp(){
ryuna 0:65b9e62cc2b6 276 /*初期化*/
ryuna 0:65b9e62cc2b6 277 Motor.baud(115200); //ボーレート設定
ryuna 0:65b9e62cc2b6 278 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:65b9e62cc2b6 279 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:65b9e62cc2b6 280
ryuna 0:65b9e62cc2b6 281
ryuna 0:65b9e62cc2b6 282 S555.calibrate(0.0005, 120.0);
ryuna 0:65b9e62cc2b6 283 S555.position(0.0); //初期位置にセット
ryuna 0:65b9e62cc2b6 284 move(0,0,0,0);//停止
ryuna 0:65b9e62cc2b6 285
ryuna 0:65b9e62cc2b6 286 Kick = 0;
ryuna 0:65b9e62cc2b6 287 Sw.mode(PullUp);
ryuna 0:65b9e62cc2b6 288
ryuna 0:65b9e62cc2b6 289
ryuna 0:65b9e62cc2b6 290 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
ryuna 0:65b9e62cc2b6 291 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
ryuna 0:65b9e62cc2b6 292 pid.setBias(PID_BIAS); //pid sed def
ryuna 0:65b9e62cc2b6 293 pid.setMode(AUTO_MODE); //pid sed def
ryuna 0:65b9e62cc2b6 294 pid.setSetPoint(REFERENCE); //pid sed def
ryuna 0:65b9e62cc2b6 295
ryuna 1:f91d53098d57 296
ryuna 0:65b9e62cc2b6 297
ryuna 0:65b9e62cc2b6 298
ryuna 0:65b9e62cc2b6 299 }
ryuna 0:65b9e62cc2b6 300 void StartLoop(){
ryuna 0:65b9e62cc2b6 301
ryuna 0:65b9e62cc2b6 302 uint8_t State = 0;
ryuna 0:65b9e62cc2b6 303 uint8_t Irnum = 0;
ryuna 0:65b9e62cc2b6 304 uint8_t LineData = 0;
ryuna 0:65b9e62cc2b6 305 while(1){
ryuna 0:65b9e62cc2b6 306 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 0:65b9e62cc2b6 307 //Lcd.cls();
ryuna 0:65b9e62cc2b6 308 State = SwRead();
ryuna 0:65b9e62cc2b6 309 if(State == 0) continue;
ryuna 0:65b9e62cc2b6 310
ryuna 0:65b9e62cc2b6 311 if(State == StartS){
ryuna 0:65b9e62cc2b6 312 /*loop end & start*/
ryuna 0:65b9e62cc2b6 313 return;
ryuna 0:65b9e62cc2b6 314 }
ryuna 0:65b9e62cc2b6 315
ryuna 0:65b9e62cc2b6 316 if(State == Debug1){
ryuna 0:65b9e62cc2b6 317 while((State == Debug1)){
ryuna 0:65b9e62cc2b6 318 LineData = (~Line+0x00) & 0x0F;
ryuna 0:65b9e62cc2b6 319 Lcd.printf("%d\n",LineData);
ryuna 0:65b9e62cc2b6 320
ryuna 0:65b9e62cc2b6 321 wait_ms(100);
ryuna 0:65b9e62cc2b6 322 State = SwRead();
ryuna 0:65b9e62cc2b6 323 }
ryuna 0:65b9e62cc2b6 324 Lcd.cls();
ryuna 0:65b9e62cc2b6 325 continue;
ryuna 0:65b9e62cc2b6 326
ryuna 0:65b9e62cc2b6 327 }
ryuna 0:65b9e62cc2b6 328 if(State == Debug2){
ryuna 0:65b9e62cc2b6 329 while((State == Debug2)){
ryuna 0:65b9e62cc2b6 330 //Irnum = IrReceiveFast();
ryuna 0:65b9e62cc2b6 331 Lcd.printf("%d\n",Irnum);
ryuna 0:65b9e62cc2b6 332 wait_ms(100);
ryuna 0:65b9e62cc2b6 333 State = SwRead();
ryuna 0:65b9e62cc2b6 334 }
ryuna 0:65b9e62cc2b6 335 Lcd.cls();
ryuna 0:65b9e62cc2b6 336 continue;
ryuna 0:65b9e62cc2b6 337 }
ryuna 0:65b9e62cc2b6 338
ryuna 0:65b9e62cc2b6 339 if(State == Debug3){
ryuna 0:65b9e62cc2b6 340 while((State == Debug3)){
ryuna 0:65b9e62cc2b6 341 //Lcd.printf("%d\n",Compass);
ryuna 0:65b9e62cc2b6 342 wait_ms(100);
ryuna 0:65b9e62cc2b6 343 State = SwRead();
ryuna 0:65b9e62cc2b6 344 }
ryuna 0:65b9e62cc2b6 345 Lcd.cls();
ryuna 0:65b9e62cc2b6 346 continue;
ryuna 0:65b9e62cc2b6 347 }
ryuna 0:65b9e62cc2b6 348
ryuna 0:65b9e62cc2b6 349 if(State == Kicker){
ryuna 0:65b9e62cc2b6 350 Led[0] = Led[1] = Led[2] = 0;
ryuna 0:65b9e62cc2b6 351 /*move(20,20,0,0);
ryuna 0:65b9e62cc2b6 352 while((State == Kicker)){
ryuna 0:65b9e62cc2b6 353 wait_ms(100);
ryuna 0:65b9e62cc2b6 354 State = SwRead();
ryuna 0:65b9e62cc2b6 355
ryuna 0:65b9e62cc2b6 356 }
ryuna 0:65b9e62cc2b6 357 move(0,0,0,0);
ryuna 0:65b9e62cc2b6 358 */
ryuna 0:65b9e62cc2b6 359 continue;
ryuna 0:65b9e62cc2b6 360 }
ryuna 0:65b9e62cc2b6 361 }
ryuna 0:65b9e62cc2b6 362
ryuna 0:65b9e62cc2b6 363 }
ryuna 0:65b9e62cc2b6 364 int main() {
ryuna 0:65b9e62cc2b6 365
ryuna 0:65b9e62cc2b6 366 /*Ir*/
ryuna 0:65b9e62cc2b6 367 uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
ryuna 0:65b9e62cc2b6 368 //uint8_t IrNumOld = 0;//過去値
ryuna 0:65b9e62cc2b6 369 /*Line*/
ryuna 0:65b9e62cc2b6 370 uint8_t LineData = 0;
ryuna 0:65b9e62cc2b6 371
ryuna 0:65b9e62cc2b6 372
ryuna 0:65b9e62cc2b6 373 /*PID補正move加算値 Revise */
ryuna 0:65b9e62cc2b6 374 double ReV = 0.0;
ryuna 0:65b9e62cc2b6 375 double SetC;
ryuna 0:65b9e62cc2b6 376
ryuna 0:65b9e62cc2b6 377 /*State */
ryuna 0:65b9e62cc2b6 378 uint8_t LineIr = 0;
ryuna 0:65b9e62cc2b6 379 uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
ryuna 0:65b9e62cc2b6 380 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
ryuna 0:65b9e62cc2b6 381 /*行動設定*/
ryuna 0:65b9e62cc2b6 382 int Power = 0;
ryuna 0:65b9e62cc2b6 383 int Rad = 0;
ryuna 0:65b9e62cc2b6 384
ryuna 0:65b9e62cc2b6 385
ryuna 0:65b9e62cc2b6 386 /*楽しい変数達*/
ryuna 0:65b9e62cc2b6 387 int nRad =0;//基礎角
ryuna 0:65b9e62cc2b6 388 int addRad = 0;//追加角
ryuna 0:65b9e62cc2b6 389
ryuna 0:65b9e62cc2b6 390
ryuna 0:65b9e62cc2b6 391
ryuna 0:65b9e62cc2b6 392 /*関数ポインタ*/
ryuna 0:65b9e62cc2b6 393 /*
ryuna 0:65b9e62cc2b6 394 void (*AnotherAction[3])(uint8_t [],double);
ryuna 0:65b9e62cc2b6 395 AnotherAction[0] = IrFrontAction;
ryuna 0:65b9e62cc2b6 396 AnotherAction[1] = IrBackAction;
ryuna 0:65b9e62cc2b6 397 AnotherAction[2] = GoHome;
ryuna 0:65b9e62cc2b6 398 */
ryuna 1:f91d53098d57 399
ryuna 0:65b9e62cc2b6 400 SetUp();/*set up routine*/
ryuna 1:f91d53098d57 401 Mbed.read_data(rx_data, ADDRESS);
ryuna 1:f91d53098d57 402 Mbed.start_read();
ryuna 1:f91d53098d57 403 //Mbed.check_rx_wait();
ryuna 0:65b9e62cc2b6 404 StartLoop(); /*loop strat, switch push end*/
ryuna 0:65b9e62cc2b6 405
ryuna 1:f91d53098d57 406 while(0){
ryuna 0:65b9e62cc2b6 407 Power = 0;
ryuna 0:65b9e62cc2b6 408 Led[0] = 1;
ryuna 0:65b9e62cc2b6 409 Rad = 0;
ryuna 0:65b9e62cc2b6 410 ReV = 0;
ryuna 0:65b9e62cc2b6 411 SetC = 0.0;
ryuna 0:65b9e62cc2b6 412
ryuna 0:65b9e62cc2b6 413 Receive();
ryuna 0:65b9e62cc2b6 414
ryuna 0:65b9e62cc2b6 415 /*白線を読んでいないか確認する*/
ryuna 0:65b9e62cc2b6 416 LineData = (~Line+0x00) & 0x0F;
ryuna 0:65b9e62cc2b6 417 if(LineData){
ryuna 0:65b9e62cc2b6 418 while(LineData){
ryuna 0:65b9e62cc2b6 419 move(0,0,0,0);//
ryuna 0:65b9e62cc2b6 420 Led[1] = Led[2] = Led[3] = 1;
ryuna 0:65b9e62cc2b6 421 LineData = (~Line+0x00) & 0x0F;
ryuna 0:65b9e62cc2b6 422 wait_ms(10);
ryuna 0:65b9e62cc2b6 423 }
ryuna 0:65b9e62cc2b6 424 Led[1] = Led[2] = Led[3] = 0;
ryuna 0:65b9e62cc2b6 425
ryuna 0:65b9e62cc2b6 426 }
ryuna 0:65b9e62cc2b6 427
ryuna 0:65b9e62cc2b6 428
ryuna 0:65b9e62cc2b6 429 Led[3] = 1;
ryuna 0:65b9e62cc2b6 430
ryuna 0:65b9e62cc2b6 431 if(IrNum == 12){
ryuna 0:65b9e62cc2b6 432 move(0,0,ReV,Rad);
ryuna 0:65b9e62cc2b6 433 continue;
ryuna 0:65b9e62cc2b6 434 }
ryuna 0:65b9e62cc2b6 435
ryuna 0:65b9e62cc2b6 436 Rad = IrRad();
ryuna 0:65b9e62cc2b6 437 nRad = wrapRad[Rad/15];
ryuna 0:65b9e62cc2b6 438 Power = 20;
ryuna 0:65b9e62cc2b6 439
ryuna 0:65b9e62cc2b6 440
ryuna 0:65b9e62cc2b6 441 Rad = nRad + addRad;
ryuna 0:65b9e62cc2b6 442
ryuna 0:65b9e62cc2b6 443 fool(&Rad,&Power);
ryuna 0:65b9e62cc2b6 444 move(Power,Power,ReV,Rad);
ryuna 0:65b9e62cc2b6 445
ryuna 0:65b9e62cc2b6 446 wait_ms(10);
ryuna 0:65b9e62cc2b6 447
ryuna 0:65b9e62cc2b6 448 Led[0] =0;
ryuna 0:65b9e62cc2b6 449
ryuna 0:65b9e62cc2b6 450
ryuna 0:65b9e62cc2b6 451 }
ryuna 0:65b9e62cc2b6 452
ryuna 1:f91d53098d57 453
ryuna 1:f91d53098d57 454 while(1){
ryuna 0:65b9e62cc2b6 455 //デモプログラム
ryuna 1:f91d53098d57 456 Receive();
ryuna 1:f91d53098d57 457 wait(0.1);
ryuna 1:f91d53098d57 458 pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
ryuna 1:f91d53098d57 459 //pc.printf("%d %d %d %d %d\n",rx_data[0],rx_data[1],rx_data[2],rx_data[0],rx_data[1]);
ryuna 0:65b9e62cc2b6 460
ryuna 0:65b9e62cc2b6 461 }
ryuna 0:65b9e62cc2b6 462
ryuna 1:f91d53098d57 463
ryuna 0:65b9e62cc2b6 464
ryuna 0:65b9e62cc2b6 465
ryuna 0:65b9e62cc2b6 466 }