CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

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

Dependencies:   AQM0802A PID Servo mbed

main.cpp@6:bfc0e8d2d3ae, 2015-03-13 (annotated)

Committer:
ryuna
Date:
Fri Mar 13 12:26:29 2015 +0000
Revision:
6:bfc0e8d2d3ae
Parent:
5:09afcbe0c18f 
???

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 4:2857f273a7f4 42
ryuna 5:09afcbe0c18f 43
ryuna 3:4a39486ff238 44 #include "Servo.h"
ryuna 0:65b9e62cc2b6 45 #include "PID.h"
ryuna 0:65b9e62cc2b6 46 #include "AQM0802A.h"
ryuna 0:65b9e62cc2b6 47 #include "main.h"
ryuna 0:65b9e62cc2b6 48
ryuna 0:65b9e62cc2b6 49
ryuna 5:09afcbe0c18f 50 void ServoWait(){
ryuna 5:09afcbe0c18f 51 WaitFlag = 0;
ryuna 5:09afcbe0c18f 52 }
ryuna 6:bfc0e8d2d3ae 53 void GoHinit(){
ryuna 6:bfc0e8d2d3ae 54 GoHFlag = 0;
ryuna 6:bfc0e8d2d3ae 55 }
ryuna 3:4a39486ff238 56
ryuna 0:65b9e62cc2b6 57
ryuna 2:e84bb87eea71 58 void move(int vr,int vl, double vs ,int Degree){
ryuna 0:65b9e62cc2b6 59 double pwm[4] = {0};
ryuna 0:65b9e62cc2b6 60 uint8_t i = 0;
ryuna 5:09afcbe0c18f 61 static int last_deg;
ryuna 5:09afcbe0c18f 62 double vias=30.0;
ryuna 5:09afcbe0c18f 63 double dif;
ryuna 5:09afcbe0c18f 64 if(abs(last_deg)<23){
ryuna 5:09afcbe0c18f 65
ryuna 5:09afcbe0c18f 66 dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
ryuna 5:09afcbe0c18f 67 if(abs(dif)<vias){
ryuna 5:09afcbe0c18f 68 pwm[0] = vr;
ryuna 5:09afcbe0c18f 69 pwm[1] = 0;
ryuna 5:09afcbe0c18f 70 pwm[2] = 0;
ryuna 5:09afcbe0c18f 71 pwm[3] = vl;
ryuna 5:09afcbe0c18f 72 }
ryuna 5:09afcbe0c18f 73 else{
ryuna 5:09afcbe0c18f 74
ryuna 5:09afcbe0c18f 75 if(dif<0){
ryuna 5:09afcbe0c18f 76 //vs = vs - (20-vias);
ryuna 5:09afcbe0c18f 77 vs = -(20+(abs(dif))*0.1);
ryuna 5:09afcbe0c18f 78 if( abs(dif) >= 100) vs = -(20+(100)*0.1);
ryuna 5:09afcbe0c18f 79 }
ryuna 5:09afcbe0c18f 80 else{
ryuna 5:09afcbe0c18f 81 //vs = vs + (20-vias);
ryuna 5:09afcbe0c18f 82 vs = (20+(abs(dif))*0.1);
ryuna 5:09afcbe0c18f 83 if( abs(dif) >= 100) vs = (20+(100)*0.1);
ryuna 5:09afcbe0c18f 84 }
ryuna 5:09afcbe0c18f 85
ryuna 5:09afcbe0c18f 86 pwm[0] = vs;
ryuna 5:09afcbe0c18f 87 pwm[1] = 0;
ryuna 5:09afcbe0c18f 88 pwm[2] = 0;
ryuna 5:09afcbe0c18f 89 pwm[3] = -vs;
ryuna 5:09afcbe0c18f 90 }
ryuna 5:09afcbe0c18f 91 }else{
ryuna 5:09afcbe0c18f 92 pwm[0] = vr;
ryuna 4:2857f273a7f4 93 pwm[1] = 0;
ryuna 4:2857f273a7f4 94 pwm[2] = 0;
ryuna 5:09afcbe0c18f 95 pwm[3] = vl;
ryuna 4:2857f273a7f4 96 }
ryuna 5:09afcbe0c18f 97
ryuna 5:09afcbe0c18f 98
ryuna 5:09afcbe0c18f 99 if(!WaitFlag){
ryuna 5:09afcbe0c18f 100 SetDegree = Degree * 11/9;
ryuna 5:09afcbe0c18f 101 if(SetDegree > 110){
ryuna 5:09afcbe0c18f 102 SetDegree = 110;
ryuna 5:09afcbe0c18f 103 }else if(SetDegree < -110){
ryuna 5:09afcbe0c18f 104 SetDegree = -110;
ryuna 5:09afcbe0c18f 105 }
ryuna 5:09afcbe0c18f 106 S555.position(SetDegree);
ryuna 5:09afcbe0c18f 107 WaitFlag = 1;
ryuna 5:09afcbe0c18f 108 STimer.attach(&ServoWait,WAIT_TIME);
ryuna 5:09afcbe0c18f 109
ryuna 5:09afcbe0c18f 110 last_deg = Degree;
ryuna 5:09afcbe0c18f 111 }
ryuna 5:09afcbe0c18f 112
ryuna 5:09afcbe0c18f 113
ryuna 0:65b9e62cc2b6 114 for(i = 0; i < 4; i++){
ryuna 0:65b9e62cc2b6 115 if(pwm[i] > 100){
ryuna 0:65b9e62cc2b6 116 pwm[i] = 100;
ryuna 0:65b9e62cc2b6 117 }else if(pwm[i] < -100){
ryuna 0:65b9e62cc2b6 118 pwm[i] = -100;
ryuna 0:65b9e62cc2b6 119 }
ryuna 0:65b9e62cc2b6 120 speed[i] = pwm[i];
ryuna 0:65b9e62cc2b6 121 }
ryuna 5:09afcbe0c18f 122
ryuna 5:09afcbe0c18f 123
ryuna 0:65b9e62cc2b6 124 wait_ms(10);
ryuna 0:65b9e62cc2b6 125 }
ryuna 2:e84bb87eea71 126
ryuna 2:e84bb87eea71 127 uint8_t PingChange(uint8_t LineData){
ryuna 2:e84bb87eea71 128 static uint8_t Last_Line;
ryuna 2:e84bb87eea71 129 static uint8_t Last_Ping;
ryuna 2:e84bb87eea71 130 uint8_t LinePing = 0;
ryuna 6:bfc0e8d2d3ae 131 uint8_t ReData =0;
ryuna 2:e84bb87eea71 132
ryuna 2:e84bb87eea71 133 if(!LineData){
ryuna 2:e84bb87eea71 134 return 0;
ryuna 2:e84bb87eea71 135 }
ryuna 2:e84bb87eea71 136
ryuna 5:09afcbe0c18f 137 if(PingData[0] <50) LinePing = LinePing + 1;
ryuna 5:09afcbe0c18f 138 if(PingData[1] <50) LinePing = LinePing + 2;
ryuna 5:09afcbe0c18f 139 if(PingData[2] <50) LinePing = LinePing + 4;
ryuna 5:09afcbe0c18f 140 if(PingData[3] <50) LinePing = LinePing + 8;
ryuna 2:e84bb87eea71 141
ryuna 2:e84bb87eea71 142
ryuna 2:e84bb87eea71 143 if(LinePing&0x01){
ryuna 2:e84bb87eea71 144 if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
ryuna 6:bfc0e8d2d3ae 145 ReData += 1;
ryuna 2:e84bb87eea71 146 }
ryuna 2:e84bb87eea71 147 }
ryuna 2:e84bb87eea71 148 if(LinePing&0x02){
ryuna 2:e84bb87eea71 149 if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
ryuna 6:bfc0e8d2d3ae 150 ReData += 2;
ryuna 2:e84bb87eea71 151 }
ryuna 2:e84bb87eea71 152 }
ryuna 2:e84bb87eea71 153 if(LinePing&0x04){
ryuna 2:e84bb87eea71 154 if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
ryuna 6:bfc0e8d2d3ae 155 ReData += 4;
ryuna 2:e84bb87eea71 156 }
ryuna 2:e84bb87eea71 157 }
ryuna 2:e84bb87eea71 158 if(LinePing&0x08){
ryuna 2:e84bb87eea71 159 if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
ryuna 6:bfc0e8d2d3ae 160 ReData += 8;
ryuna 2:e84bb87eea71 161 }
ryuna 2:e84bb87eea71 162 }
ryuna 6:bfc0e8d2d3ae 163 if(ReData){
ryuna 6:bfc0e8d2d3ae 164 Last_Ping = LinePing;
ryuna 6:bfc0e8d2d3ae 165 Last_Line = LineData;
ryuna 6:bfc0e8d2d3ae 166 return ReData;
ryuna 6:bfc0e8d2d3ae 167 }
ryuna 2:e84bb87eea71 168 Last_Ping = 0;
ryuna 2:e84bb87eea71 169 Last_Line = 0;
ryuna 2:e84bb87eea71 170 return 0;
ryuna 2:e84bb87eea71 171
ryuna 2:e84bb87eea71 172 }
ryuna 2:e84bb87eea71 173
ryuna 2:e84bb87eea71 174 void fool (int *Degree, int *Power){
ryuna 2:e84bb87eea71 175 static int Last_Degree = 0;
ryuna 0:65b9e62cc2b6 176 static int Last_Vector = 1;
ryuna 2:e84bb87eea71 177 int degree = *Degree;
ryuna 0:65b9e62cc2b6 178 int Temp;
ryuna 2:e84bb87eea71 179
ryuna 2:e84bb87eea71 180
ryuna 2:e84bb87eea71 181 if((*Degree <0)||(*Degree >=360)){
ryuna 2:e84bb87eea71 182 *Degree = 0;
ryuna 2:e84bb87eea71 183 Last_Degree = 0;
ryuna 2:e84bb87eea71 184 Last_Vector = 1;
ryuna 2:e84bb87eea71 185 return ;
ryuna 2:e84bb87eea71 186 }
ryuna 2:e84bb87eea71 187
ryuna 2:e84bb87eea71 188 Temp = Last_Degree % 180;
ryuna 5:09afcbe0c18f 189 if((Temp>70) &&(Temp<110)){
ryuna 2:e84bb87eea71 190 Temp = *Degree % 180;
ryuna 5:09afcbe0c18f 191 if((Temp>70) &&(Temp<110)){
ryuna 2:e84bb87eea71 192 Temp = abs(*Degree - Last_Degree);
ryuna 0:65b9e62cc2b6 193 if(Temp>160){
ryuna 0:65b9e62cc2b6 194 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
ryuna 2:e84bb87eea71 195 if(*Degree/180){
ryuna 2:e84bb87eea71 196 *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
ryuna 0:65b9e62cc2b6 197 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 198 }else{
ryuna 2:e84bb87eea71 199 *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
ryuna 0:65b9e62cc2b6 200 *Power = *Power * Last_Vector;
ryuna 0:65b9e62cc2b6 201 }
ryuna 2:e84bb87eea71 202 Last_Degree = degree;
ryuna 2:e84bb87eea71 203 if((*Degree <= -120)||(*Degree >=120)){
ryuna 2:e84bb87eea71 204 *Degree = 0;
ryuna 2:e84bb87eea71 205 }
ryuna 0:65b9e62cc2b6 206 return;
ryuna 0:65b9e62cc2b6 207 }else if((Last_Vector+2) == 1){
ryuna 0:65b9e62cc2b6 208 /*逆転のまま角度拡張*/
ryuna 2:e84bb87eea71 209 if(*Degree/180){
ryuna 2:e84bb87eea71 210 *Degree = -360 + *Degree ;
ryuna 2:e84bb87eea71 211
ryuna 0:65b9e62cc2b6 212 }
ryuna 2:e84bb87eea71 213 *Power = *Power * Last_Vector;
ryuna 2:e84bb87eea71 214 Last_Degree = degree;
ryuna 2:e84bb87eea71 215 if((*Degree <= -120)||(*Degree >=120)){
ryuna 2:e84bb87eea71 216 *Degree = 0;
ryuna 2:e84bb87eea71 217
ryuna 2:e84bb87eea71 218 }
ryuna 0:65b9e62cc2b6 219 return;
ryuna 0:65b9e62cc2b6 220
ryuna 0:65b9e62cc2b6 221 }else if((Last_Vector+2) == 3){
ryuna 0:65b9e62cc2b6 222 /*正転のまま*/
ryuna 2:e84bb87eea71 223 if(*Degree/180){
ryuna 2:e84bb87eea71 224 *Degree = -360 + *Degree ;
ryuna 0:65b9e62cc2b6 225 }
ryuna 2:e84bb87eea71 226 *Power = *Power * Last_Vector;
ryuna 2:e84bb87eea71 227 Last_Degree = degree;
ryuna 2:e84bb87eea71 228 if((*Degree <= -120)||(*Degree >=120)){
ryuna 2:e84bb87eea71 229 *Degree = 0;
ryuna 2:e84bb87eea71 230 }
ryuna 0:65b9e62cc2b6 231 return;
ryuna 0:65b9e62cc2b6 232 }
ryuna 0:65b9e62cc2b6 233 }
ryuna 0:65b9e62cc2b6 234 }
ryuna 0:65b9e62cc2b6 235 /*通常動作*/
ryuna 2:e84bb87eea71 236 if(*Degree == 0){
ryuna 2:e84bb87eea71 237 Last_Vector = DegreeToVector[0];
ryuna 2:e84bb87eea71 238 *Degree = Angle[*Degree%180];
ryuna 2:e84bb87eea71 239 *Power = *Power * Last_Vector;
ryuna 2:e84bb87eea71 240 Last_Degree = degree;
ryuna 2:e84bb87eea71 241 return ;
ryuna 2:e84bb87eea71 242 }
ryuna 5:09afcbe0c18f 243 if(*Degree == 270){
ryuna 5:09afcbe0c18f 244 Last_Vector = DegreeToVector[(*Degree)/90];
ryuna 5:09afcbe0c18f 245 *Degree = -90;
ryuna 5:09afcbe0c18f 246 *Power = *Power * Last_Vector;
ryuna 5:09afcbe0c18f 247 Last_Degree = degree;
ryuna 5:09afcbe0c18f 248 return;
ryuna 5:09afcbe0c18f 249 }
ryuna 2:e84bb87eea71 250 Last_Vector = DegreeToVector[(*Degree-1)/90];
ryuna 2:e84bb87eea71 251 *Degree = Angle[*Degree%180];
ryuna 0:65b9e62cc2b6 252 *Power = *Power * Last_Vector;
ryuna 2:e84bb87eea71 253 Last_Degree = degree;
ryuna 2:e84bb87eea71 254 if((*Degree <= -120)||(*Degree >=120)){
ryuna 2:e84bb87eea71 255 *Degree = 0;
ryuna 2:e84bb87eea71 256 }
ryuna 0:65b9e62cc2b6 257
ryuna 0:65b9e62cc2b6 258 }
ryuna 2:e84bb87eea71 259 int IrDegree(){
ryuna 0:65b9e62cc2b6 260 /*irの1位の値，2位の場所からirの360へ持っていく*/
ryuna 0:65b9e62cc2b6 261
ryuna 2:e84bb87eea71 262 uint8_t IrF ,IrS;
ryuna 2:e84bb87eea71 263 unsigned int irdegree = 0;
ryuna 2:e84bb87eea71 264 if(IrNum >=12){
ryuna 2:e84bb87eea71 265 return 0;
ryuna 2:e84bb87eea71 266 }
ryuna 2:e84bb87eea71 267 IrF = IrData[0]/12;
ryuna 2:e84bb87eea71 268 IrS = IrData[0]%12;
ryuna 2:e84bb87eea71 269
ryuna 2:e84bb87eea71 270 if(IrF == 0 ){
ryuna 2:e84bb87eea71 271 if(IrS == 11){
ryuna 2:e84bb87eea71 272 irdegree = 15;
ryuna 0:65b9e62cc2b6 273
ryuna 2:e84bb87eea71 274 }else if(IrS == 1){
ryuna 2:e84bb87eea71 275 irdegree = 345;
ryuna 2:e84bb87eea71 276 }
ryuna 2:e84bb87eea71 277 return irdegree;
ryuna 2:e84bb87eea71 278 }
ryuna 2:e84bb87eea71 279 irdegree = 360 - IrF*30;
ryuna 2:e84bb87eea71 280 if(IrS&&(abs(IrF-IrS) == 1)){
ryuna 2:e84bb87eea71 281 irdegree = irdegree - (IrF - IrS)*15;
ryuna 0:65b9e62cc2b6 282
ryuna 0:65b9e62cc2b6 283 }
ryuna 2:e84bb87eea71 284 if(irdegree>=360){
ryuna 2:e84bb87eea71 285 return 0;
ryuna 2:e84bb87eea71 286 }
ryuna 2:e84bb87eea71 287
ryuna 2:e84bb87eea71 288 return irdegree;
ryuna 0:65b9e62cc2b6 289 }
ryuna 0:65b9e62cc2b6 290
ryuna 0:65b9e62cc2b6 291
ryuna 2:e84bb87eea71 292 void IrFrontAction()//ball 12 or 0 o-clock
ryuna 0:65b9e62cc2b6 293 {
ryuna 6:bfc0e8d2d3ae 294 if(IrData[1]>65){
ryuna 2:e84bb87eea71 295 move(30,30,CompassPID,0);
ryuna 2:e84bb87eea71 296 return;
ryuna 2:e84bb87eea71 297 }
ryuna 6:bfc0e8d2d3ae 298 if(IrData[1]>57){
ryuna 5:09afcbe0c18f 299 move(20,20,CompassPID,0);
ryuna 2:e84bb87eea71 300 return;
ryuna 2:e84bb87eea71 301 }
ryuna 2:e84bb87eea71 302 /*IrData[1]>500*/
ryuna 5:09afcbe0c18f 303 move(10,10,0,0);
ryuna 6:bfc0e8d2d3ae 304 if(PingData[0]<60){
ryuna 2:e84bb87eea71 305 if((PingData[1]<60)&&(PingData[1]>40)){
ryuna 2:e84bb87eea71 306 /*右側に居る*/
ryuna 6:bfc0e8d2d3ae 307 if(IrData[1]<50){
ryuna 6:bfc0e8d2d3ae 308 move(40,25,0,10);
ryuna 6:bfc0e8d2d3ae 309 Kick = 1;
ryuna 6:bfc0e8d2d3ae 310 wait_ms(200);
ryuna 6:bfc0e8d2d3ae 311 move(30,30,0,0);
ryuna 6:bfc0e8d2d3ae 312 Kick = 0;
ryuna 6:bfc0e8d2d3ae 313 return;
ryuna 6:bfc0e8d2d3ae 314 }
ryuna 6:bfc0e8d2d3ae 315
ryuna 5:09afcbe0c18f 316 move(40,25,0,10);
ryuna 5:09afcbe0c18f 317 wait_ms(200);
ryuna 2:e84bb87eea71 318 move(30,30,0,0);
ryuna 2:e84bb87eea71 319 return;
ryuna 6:bfc0e8d2d3ae 320
ryuna 2:e84bb87eea71 321 }
ryuna 2:e84bb87eea71 322 if((PingData[1]<60)&&(PingData[1]>40)){
ryuna 2:e84bb87eea71 323 /*左側に居る*/
ryuna 6:bfc0e8d2d3ae 324 if(IrData[1]<50){
ryuna 6:bfc0e8d2d3ae 325 move(25,40,0,-10);
ryuna 6:bfc0e8d2d3ae 326 Kick = 1;
ryuna 6:bfc0e8d2d3ae 327 wait_ms(200);
ryuna 6:bfc0e8d2d3ae 328 move(30,30,0,0);
ryuna 6:bfc0e8d2d3ae 329 Kick = 0;
ryuna 6:bfc0e8d2d3ae 330 return;
ryuna 6:bfc0e8d2d3ae 331 }
ryuna 5:09afcbe0c18f 332 move(25,40,0,-10);
ryuna 5:09afcbe0c18f 333 wait_ms(200);
ryuna 2:e84bb87eea71 334 move(30,30,0,0);
ryuna 2:e84bb87eea71 335 return;
ryuna 6:bfc0e8d2d3ae 336
ryuna 2:e84bb87eea71 337 }
ryuna 2:e84bb87eea71 338 /*それ以外*/
ryuna 5:09afcbe0c18f 339
ryuna 4:2857f273a7f4 340 //Receive();
ryuna 2:e84bb87eea71 341 if(!(IrNum == 0)) return;
ryuna 2:e84bb87eea71 342 move(40,40,CompassPID,0);
ryuna 5:09afcbe0c18f 343 Kick = 1;
ryuna 5:09afcbe0c18f 344 wait_ms(100);
ryuna 5:09afcbe0c18f 345 Kick = 0;
ryuna 2:e84bb87eea71 346 return ;
ryuna 2:e84bb87eea71 347 }
ryuna 2:e84bb87eea71 348 move(20,20,CompassPID,0);
ryuna 2:e84bb87eea71 349
ryuna 2:e84bb87eea71 350
ryuna 0:65b9e62cc2b6 351
ryuna 0:65b9e62cc2b6 352 }
ryuna 0:65b9e62cc2b6 353
ryuna 2:e84bb87eea71 354 void IrBackAction()//ball found six o-clock
ryuna 0:65b9e62cc2b6 355 {
ryuna 0:65b9e62cc2b6 356 /***
ryuna 0:65b9e62cc2b6 357 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
ryuna 0:65b9e62cc2b6 358 *
ryuna 0:65b9e62cc2b6 359 **/
ryuna 6:bfc0e8d2d3ae 360 int Pingrl = PingData[1] + PingData[3];
ryuna 6:bfc0e8d2d3ae 361 if(Pingrl >=120){
ryuna 6:bfc0e8d2d3ae 362 if(PingData[1]>PingData[3]){
ryuna 6:bfc0e8d2d3ae 363 /*右が大きい*/
ryuna 6:bfc0e8d2d3ae 364 if(IrData[1]>65){
ryuna 6:bfc0e8d2d3ae 365 move(-25,-25,CompassPID,45);
ryuna 6:bfc0e8d2d3ae 366 return;
ryuna 6:bfc0e8d2d3ae 367 }
ryuna 6:bfc0e8d2d3ae 368 if(IrData[1]>55){
ryuna 6:bfc0e8d2d3ae 369 move(-20,-20,CompassPID,60);
ryuna 6:bfc0e8d2d3ae 370 return;
ryuna 6:bfc0e8d2d3ae 371 }
ryuna 6:bfc0e8d2d3ae 372 move(-20,-20,CompassPID,90);
ryuna 2:e84bb87eea71 373 return;
ryuna 2:e84bb87eea71 374 }
ryuna 6:bfc0e8d2d3ae 375 /*左が大きい*/
ryuna 6:bfc0e8d2d3ae 376
ryuna 6:bfc0e8d2d3ae 377 if(IrData[1]>65){
ryuna 6:bfc0e8d2d3ae 378 move(-25,-25,CompassPID,-45);
ryuna 6:bfc0e8d2d3ae 379 return;
ryuna 6:bfc0e8d2d3ae 380 }
ryuna 6:bfc0e8d2d3ae 381 if(IrData[1]>55){
ryuna 6:bfc0e8d2d3ae 382 move(-20,-20,CompassPID,-60);
ryuna 6:bfc0e8d2d3ae 383 return;
ryuna 6:bfc0e8d2d3ae 384 }
ryuna 6:bfc0e8d2d3ae 385 move(-20,-20,CompassPID,-90);
ryuna 6:bfc0e8d2d3ae 386 return;
ryuna 6:bfc0e8d2d3ae 387 }
ryuna 6:bfc0e8d2d3ae 388
ryuna 6:bfc0e8d2d3ae 389 if(PingData[1]>PingData[3]){
ryuna 6:bfc0e8d2d3ae 390 /*右が大きい*/
ryuna 6:bfc0e8d2d3ae 391 if(IrData[1]>65){
ryuna 6:bfc0e8d2d3ae 392 move(-25,-25,0,30);
ryuna 6:bfc0e8d2d3ae 393 return;
ryuna 6:bfc0e8d2d3ae 394 }
ryuna 6:bfc0e8d2d3ae 395 if(IrData[1]>55){
ryuna 6:bfc0e8d2d3ae 396 move(-20,-20,0,60);
ryuna 2:e84bb87eea71 397 return;
ryuna 2:e84bb87eea71 398 }
ryuna 6:bfc0e8d2d3ae 399 move(-15,-15,0,70);
ryuna 0:65b9e62cc2b6 400 return;
ryuna 0:65b9e62cc2b6 401 }
ryuna 0:65b9e62cc2b6 402 /*左が大きい*/
ryuna 0:65b9e62cc2b6 403
ryuna 6:bfc0e8d2d3ae 404 if(IrData[1]>65){
ryuna 6:bfc0e8d2d3ae 405 move(-25,-25,CompassPID,-30);
ryuna 2:e84bb87eea71 406 return;
ryuna 0:65b9e62cc2b6 407 }
ryuna 6:bfc0e8d2d3ae 408 if(IrData[1]>55){
ryuna 2:e84bb87eea71 409 move(-20,-20,CompassPID,-60);
ryuna 2:e84bb87eea71 410 return;
ryuna 2:e84bb87eea71 411 }
ryuna 6:bfc0e8d2d3ae 412 move(-15,-15,CompassPID,-70);
ryuna 2:e84bb87eea71 413 return;
ryuna 0:65b9e62cc2b6 414
ryuna 2:e84bb87eea71 415 }
ryuna 2:e84bb87eea71 416
ryuna 2:e84bb87eea71 417 void GoHome()//Ball is not found.
ryuna 2:e84bb87eea71 418 {
ryuna 5:09afcbe0c18f 419 int pingRl =0;
ryuna 5:09afcbe0c18f 420 int PowLev;
ryuna 6:bfc0e8d2d3ae 421 if(!GoHFlag){
ryuna 6:bfc0e8d2d3ae 422 GoHFlag = 1;
ryuna 6:bfc0e8d2d3ae 423 GoHTimer.attach(&GoHinit,1.0);
ryuna 6:bfc0e8d2d3ae 424 move(0,0,0,0);
ryuna 6:bfc0e8d2d3ae 425 wait_ms(100);
ryuna 6:bfc0e8d2d3ae 426
ryuna 6:bfc0e8d2d3ae 427 }
ryuna 5:09afcbe0c18f 428 if(PingData[2] >=50){//後ろからの距離60cm
ryuna 6:bfc0e8d2d3ae 429 move(-25,-25,0,0);
ryuna 5:09afcbe0c18f 430 return ;
ryuna 5:09afcbe0c18f 431 }
ryuna 6:bfc0e8d2d3ae 432 if(PingData[2] <= 25){
ryuna 6:bfc0e8d2d3ae 433 move(15,15,0,0);
ryuna 0:65b9e62cc2b6 434 return ;
ryuna 5:09afcbe0c18f 435 }
ryuna 5:09afcbe0c18f 436 pingRl = PingData[1]+PingData[3];
ryuna 6:bfc0e8d2d3ae 437 if(pingRl <120){
ryuna 6:bfc0e8d2d3ae 438 move(0,0,0,0);//stop
ryuna 5:09afcbe0c18f 439 return;
ryuna 5:09afcbe0c18f 440 }
ryuna 6:bfc0e8d2d3ae 441 if(PingData[1] < 70){
ryuna 6:bfc0e8d2d3ae 442 PowLev = (70 - PingData[1]);
ryuna 5:09afcbe0c18f 443 move(PowLev,PowLev,0,90);
ryuna 5:09afcbe0c18f 444 return ;
ryuna 6:bfc0e8d2d3ae 445 }else if (PingData[3] < 70){
ryuna 6:bfc0e8d2d3ae 446 PowLev = (70 - PingData[3]);
ryuna 5:09afcbe0c18f 447 move(PowLev,PowLev,0,-90);
ryuna 5:09afcbe0c18f 448 return ;
ryuna 5:09afcbe0c18f 449 }
ryuna 5:09afcbe0c18f 450
ryuna 5:09afcbe0c18f 451 move(0,0,0,0);//stop
ryuna 2:e84bb87eea71 452
ryuna 0:65b9e62cc2b6 453 }
ryuna 0:65b9e62cc2b6 454
ryuna 0:65b9e62cc2b6 455
ryuna 0:65b9e62cc2b6 456 void PidUpdate()
ryuna 0:65b9e62cc2b6 457 {
ryuna 5:09afcbe0c18f 458
ryuna 5:09afcbe0c18f 459 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 5:09afcbe0c18f 460 InputPID = ((Compass/10) + 540 - CompassDef) % 360;
ryuna 5:09afcbe0c18f 461
ryuna 5:09afcbe0c18f 462 pid.setProcessValue(InputPID);
ryuna 5:09afcbe0c18f 463 CompassPID = (pid.compute());
ryuna 5:09afcbe0c18f 464 /*
ryuna 2:e84bb87eea71 465 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 2:e84bb87eea71 466 InputPID = Compass;
ryuna 2:e84bb87eea71 467
ryuna 2:e84bb87eea71 468 pid.setProcessValue(InputPID);
ryuna 4:2857f273a7f4 469 CompassPID = (pid.compute());
ryuna 5:09afcbe0c18f 470 */
ryuna 0:65b9e62cc2b6 471 }
ryuna 0:65b9e62cc2b6 472
ryuna 0:65b9e62cc2b6 473
ryuna 0:65b9e62cc2b6 474
ryuna 0:65b9e62cc2b6 475
ryuna 0:65b9e62cc2b6 476
ryuna 0:65b9e62cc2b6 477
ryuna 0:65b9e62cc2b6 478
ryuna 0:65b9e62cc2b6 479 uint8_t SwRead(){
ryuna 0:65b9e62cc2b6 480 /******
ryuna 0:65b9e62cc2b6 481 *retrun : sw_state
ryuna 0:65b9e62cc2b6 482 *StartS = 0x01;
ryuna 0:65b9e62cc2b6 483 *Debug2 = 0x02;
ryuna 0:65b9e62cc2b6 484 *Debug1 = 0x04;
ryuna 0:65b9e62cc2b6 485 *Debug3 = 0x06;
ryuna 0:65b9e62cc2b6 486 *Kicker = 0x08;
ryuna 0:65b9e62cc2b6 487 *Calibration = 0x10;
ryuna 0:65b9e62cc2b6 488
ryuna 0:65b9e62cc2b6 489 *
ryuna 0:65b9e62cc2b6 490 *****/
ryuna 0:65b9e62cc2b6 491 uint8_t i,temp,temp2;
ryuna 0:65b9e62cc2b6 492 temp = ~Sw - 224;
ryuna 0:65b9e62cc2b6 493 if(!(temp == Kicker
ryuna 0:65b9e62cc2b6 494 ||temp == Debug1
ryuna 0:65b9e62cc2b6 495 ||temp == Debug2
ryuna 0:65b9e62cc2b6 496 ||temp == Debug3
ryuna 5:09afcbe0c18f 497 ||temp == Debug12
ryuna 5:09afcbe0c18f 498 ||temp == Debug23
ryuna 0:65b9e62cc2b6 499 ||temp == StartS)) return 0;/*スイッチが押されていない*/
ryuna 0:65b9e62cc2b6 500 if(!(temp == 0x00)){
ryuna 0:65b9e62cc2b6 501 for(i = 0; i < 50; i++);
ryuna 0:65b9e62cc2b6 502 temp2 = ~Sw - 224;
ryuna 0:65b9e62cc2b6 503 if(temp == temp2){
ryuna 0:65b9e62cc2b6 504 return temp;
ryuna 0:65b9e62cc2b6 505 }
ryuna 0:65b9e62cc2b6 506 }
ryuna 0:65b9e62cc2b6 507 return 0;
ryuna 0:65b9e62cc2b6 508 }
ryuna 0:65b9e62cc2b6 509
ryuna 0:65b9e62cc2b6 510 //通信（モータ用）
ryuna 0:65b9e62cc2b6 511 void tx_motor(){
ryuna 0:65b9e62cc2b6 512 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:65b9e62cc2b6 513 Motor.printf("%s",StringFIN.c_str());
ryuna 0:65b9e62cc2b6 514 }
ryuna 0:65b9e62cc2b6 515
ryuna 0:65b9e62cc2b6 516 void SetUp(){
ryuna 0:65b9e62cc2b6 517 /*初期化*/
ryuna 3:4a39486ff238 518
ryuna 0:65b9e62cc2b6 519 Motor.baud(115200); //ボーレート設定
ryuna 0:65b9e62cc2b6 520 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:65b9e62cc2b6 521 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:65b9e62cc2b6 522
ryuna 4:2857f273a7f4 523 Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
ryuna 4:2857f273a7f4 524
ryuna 5:09afcbe0c18f 525 S555.calibrate(0.0006, 120.0);
ryuna 2:e84bb87eea71 526 //S555.position(0.0); //初期位置にセット
ryuna 0:65b9e62cc2b6 527 move(0,0,0,0);//停止
ryuna 0:65b9e62cc2b6 528
ryuna 5:09afcbe0c18f 529
ryuna 0:65b9e62cc2b6 530 Kick = 0;
ryuna 5:09afcbe0c18f 531
ryuna 0:65b9e62cc2b6 532 Sw.mode(PullUp);
ryuna 0:65b9e62cc2b6 533
ryuna 0:65b9e62cc2b6 534 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
ryuna 0:65b9e62cc2b6 535 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
ryuna 0:65b9e62cc2b6 536 pid.setBias(PID_BIAS); //pid sed def
ryuna 0:65b9e62cc2b6 537 pid.setMode(AUTO_MODE); //pid sed def
ryuna 0:65b9e62cc2b6 538 pid.setSetPoint(REFERENCE); //pid sed def
ryuna 1:f91d53098d57 539
ryuna 0:65b9e62cc2b6 540
ryuna 5:09afcbe0c18f 541 for(int i=0; i<15; i++){
ryuna 5:09afcbe0c18f 542 CompassDef = Compass/10;
ryuna 5:09afcbe0c18f 543 wait_ms(10);
ryuna 5:09afcbe0c18f 544 }
ryuna 5:09afcbe0c18f 545 //pidupdate.attach(&PidUpdate, PID_CYCLE);
ryuna 5:09afcbe0c18f 546
ryuna 0:65b9e62cc2b6 547 }
ryuna 0:65b9e62cc2b6 548 void StartLoop(){
ryuna 0:65b9e62cc2b6 549
ryuna 0:65b9e62cc2b6 550 uint8_t State = 0;
ryuna 0:65b9e62cc2b6 551 uint8_t LineData = 0;
ryuna 5:09afcbe0c18f 552 int a;
ryuna 0:65b9e62cc2b6 553 while(1){
ryuna 0:65b9e62cc2b6 554 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 0:65b9e62cc2b6 555 //Lcd.cls();
ryuna 2:e84bb87eea71 556
ryuna 0:65b9e62cc2b6 557 State = SwRead();
ryuna 0:65b9e62cc2b6 558 if(State == 0) continue;
ryuna 0:65b9e62cc2b6 559
ryuna 0:65b9e62cc2b6 560 if(State == StartS){
ryuna 0:65b9e62cc2b6 561 /*loop end & start*/
ryuna 0:65b9e62cc2b6 562 return;
ryuna 0:65b9e62cc2b6 563 }
ryuna 0:65b9e62cc2b6 564
ryuna 0:65b9e62cc2b6 565 if(State == Debug1){
ryuna 0:65b9e62cc2b6 566 while((State == Debug1)){
ryuna 0:65b9e62cc2b6 567 LineData = (~Line+0x00) & 0x0F;
ryuna 0:65b9e62cc2b6 568 Lcd.printf("%d\n",LineData);
ryuna 0:65b9e62cc2b6 569
ryuna 0:65b9e62cc2b6 570 wait_ms(100);
ryuna 0:65b9e62cc2b6 571 State = SwRead();
ryuna 0:65b9e62cc2b6 572 }
ryuna 0:65b9e62cc2b6 573 Lcd.cls();
ryuna 0:65b9e62cc2b6 574 continue;
ryuna 0:65b9e62cc2b6 575
ryuna 0:65b9e62cc2b6 576 }
ryuna 0:65b9e62cc2b6 577 if(State == Debug2){
ryuna 0:65b9e62cc2b6 578 while((State == Debug2)){
ryuna 4:2857f273a7f4 579 //Receive();
ryuna 6:bfc0e8d2d3ae 580 Lcd.printf("%d\t%d\n",IrNum,IrData[1]);
ryuna 0:65b9e62cc2b6 581 wait_ms(100);
ryuna 0:65b9e62cc2b6 582 State = SwRead();
ryuna 0:65b9e62cc2b6 583 }
ryuna 0:65b9e62cc2b6 584 Lcd.cls();
ryuna 0:65b9e62cc2b6 585 continue;
ryuna 0:65b9e62cc2b6 586 }
ryuna 0:65b9e62cc2b6 587
ryuna 0:65b9e62cc2b6 588 if(State == Debug3){
ryuna 0:65b9e62cc2b6 589 while((State == Debug3)){
ryuna 5:09afcbe0c18f 590 a = (Compass/10 + 540 - CompassDef) % 360;
ryuna 5:09afcbe0c18f 591 Lcd.printf("%d\n",a);
ryuna 0:65b9e62cc2b6 592 wait_ms(100);
ryuna 0:65b9e62cc2b6 593 State = SwRead();
ryuna 0:65b9e62cc2b6 594 }
ryuna 0:65b9e62cc2b6 595 Lcd.cls();
ryuna 0:65b9e62cc2b6 596 continue;
ryuna 0:65b9e62cc2b6 597 }
ryuna 5:09afcbe0c18f 598 if(State == Debug12){
ryuna 5:09afcbe0c18f 599 while((State == Debug12)){
ryuna 5:09afcbe0c18f 600 Lcd.printf("%d %d\n%d %d\n",PingData[0],PingData[1],PingData[2],PingData[3]);
ryuna 5:09afcbe0c18f 601 wait_ms(100);
ryuna 5:09afcbe0c18f 602 State = SwRead();
ryuna 5:09afcbe0c18f 603 }
ryuna 5:09afcbe0c18f 604 Lcd.cls();
ryuna 5:09afcbe0c18f 605 continue;
ryuna 5:09afcbe0c18f 606 }
ryuna 0:65b9e62cc2b6 607 if(State == Kicker){
ryuna 0:65b9e62cc2b6 608 Led[0] = Led[1] = Led[2] = 0;
ryuna 4:2857f273a7f4 609 Kick = 1;
ryuna 4:2857f273a7f4 610 wait_ms(500);
ryuna 4:2857f273a7f4 611 Kick = 0;
ryuna 0:65b9e62cc2b6 612 while((State == Kicker)){
ryuna 0:65b9e62cc2b6 613 wait_ms(100);
ryuna 0:65b9e62cc2b6 614 State = SwRead();
ryuna 0:65b9e62cc2b6 615 }
ryuna 0:65b9e62cc2b6 616 continue;
ryuna 0:65b9e62cc2b6 617 }
ryuna 0:65b9e62cc2b6 618 }
ryuna 0:65b9e62cc2b6 619
ryuna 0:65b9e62cc2b6 620 }
ryuna 0:65b9e62cc2b6 621 int main() {
ryuna 0:65b9e62cc2b6 622
ryuna 0:65b9e62cc2b6 623 //uint8_t IrNumOld = 0;//過去値
ryuna 0:65b9e62cc2b6 624 /*Line*/
ryuna 0:65b9e62cc2b6 625 uint8_t LineData = 0;
ryuna 2:e84bb87eea71 626 uint8_t LinePing = 0;
ryuna 0:65b9e62cc2b6 627
ryuna 2:e84bb87eea71 628
ryuna 0:65b9e62cc2b6 629 /*State */
ryuna 0:65b9e62cc2b6 630 uint8_t LineIr = 0;
ryuna 5:09afcbe0c18f 631 uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
ryuna 3:4a39486ff238 632
ryuna 5:09afcbe0c18f 633 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
ryuna 2:e84bb87eea71 634
ryuna 0:65b9e62cc2b6 635 /*行動設定*/
ryuna 0:65b9e62cc2b6 636 int Power = 0;
ryuna 2:e84bb87eea71 637 int Degree = 0;
ryuna 0:65b9e62cc2b6 638
ryuna 0:65b9e62cc2b6 639 /*楽しい変数達*/
ryuna 2:e84bb87eea71 640 int nDegree =0;//基礎角
ryuna 2:e84bb87eea71 641 int addDegree = 0;//追加角
ryuna 5:09afcbe0c18f 642 int Temp = 0;
ryuna 0:65b9e62cc2b6 643
ryuna 5:09afcbe0c18f 644 int StopCt = 0;
ryuna 0:65b9e62cc2b6 645
ryuna 0:65b9e62cc2b6 646 /*関数ポインタ*/
ryuna 2:e84bb87eea71 647
ryuna 2:e84bb87eea71 648
ryuna 2:e84bb87eea71 649 //void (*AnotherAction[3])(uint8_t [],double);
ryuna 2:e84bb87eea71 650 void (*AnotherAction[3])();
ryuna 0:65b9e62cc2b6 651 AnotherAction[0] = IrFrontAction;
ryuna 0:65b9e62cc2b6 652 AnotherAction[1] = IrBackAction;
ryuna 0:65b9e62cc2b6 653 AnotherAction[2] = GoHome;
ryuna 1:f91d53098d57 654
ryuna 2:e84bb87eea71 655
ryuna 0:65b9e62cc2b6 656 SetUp();/*set up routine*/
ryuna 4:2857f273a7f4 657
ryuna 0:65b9e62cc2b6 658 StartLoop(); /*loop strat, switch push end*/
ryuna 2:e84bb87eea71 659 Led[0] = Led[1] = Led[2] = Led[3] = 0;
ryuna 2:e84bb87eea71 660 wait_ms(100);
ryuna 0:65b9e62cc2b6 661
ryuna 4:2857f273a7f4 662 while(1){
ryuna 0:65b9e62cc2b6 663
ryuna 5:09afcbe0c18f 664 //S555.calibrate(0.0006, 120.0);
ryuna 2:e84bb87eea71 665
ryuna 4:2857f273a7f4 666 //Receive();
ryuna 2:e84bb87eea71 667 //Lcd.printf("%d\n",IrNum);
ryuna 0:65b9e62cc2b6 668 /*白線を読んでいないか確認する*/
ryuna 5:09afcbe0c18f 669
ryuna 0:65b9e62cc2b6 670 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 671
ryuna 0:65b9e62cc2b6 672 if(LineData){
ryuna 2:e84bb87eea71 673 LineIr = LineData & IrChange[IrNum];
ryuna 2:e84bb87eea71 674 LinePing = PingChange(LineData);
ryuna 2:e84bb87eea71 675 if(LineIr){
ryuna 2:e84bb87eea71 676 move(0,0,0,0);
ryuna 5:09afcbe0c18f 677 StopCt = 0;
ryuna 2:e84bb87eea71 678 while(LineIr){
ryuna 5:09afcbe0c18f 679 if(StopCt>30){
ryuna 5:09afcbe0c18f 680 GoHome();
ryuna 5:09afcbe0c18f 681 wait_ms(100);
ryuna 5:09afcbe0c18f 682 }
ryuna 5:09afcbe0c18f 683 StopCt++;
ryuna 2:e84bb87eea71 684 Led[1] = Led[2] = Led[3] = 1;
ryuna 6:bfc0e8d2d3ae 685
ryuna 2:e84bb87eea71 686 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 687 LineIr = LineData & IrChange[IrNum];
ryuna 2:e84bb87eea71 688 wait_ms(10);
ryuna 2:e84bb87eea71 689 }
ryuna 2:e84bb87eea71 690 }else if(LinePing){
ryuna 3:4a39486ff238 691 move(0,0,0,0);
ryuna 5:09afcbe0c18f 692 StopCt = 0;
ryuna 2:e84bb87eea71 693 while(LinePing){
ryuna 5:09afcbe0c18f 694 if(StopCt>30){
ryuna 5:09afcbe0c18f 695 GoHome();
ryuna 6:bfc0e8d2d3ae 696 wait_ms(300);
ryuna 5:09afcbe0c18f 697 }
ryuna 5:09afcbe0c18f 698 StopCt++;
ryuna 2:e84bb87eea71 699 Led[1] = Led[2] = Led[3] = 1;
ryuna 6:bfc0e8d2d3ae 700
ryuna 6:bfc0e8d2d3ae 701 if((LinePing&0x06)||(LinePing&0x0C)){
ryuna 6:bfc0e8d2d3ae 702 if(LinePing&0x06){
ryuna 6:bfc0e8d2d3ae 703 move(15,15,0,45);
ryuna 6:bfc0e8d2d3ae 704 }else if(LinePing&0x0C){
ryuna 6:bfc0e8d2d3ae 705 move(15,15,0,-45);
ryuna 6:bfc0e8d2d3ae 706 }
ryuna 6:bfc0e8d2d3ae 707 wait_ms(200);
ryuna 6:bfc0e8d2d3ae 708 }
ryuna 2:e84bb87eea71 709 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 710 LinePing = PingChange(LineData);
ryuna 3:4a39486ff238 711
ryuna 2:e84bb87eea71 712 wait_ms(10);
ryuna 2:e84bb87eea71 713 }
ryuna 0:65b9e62cc2b6 714 }
ryuna 2:e84bb87eea71 715
ryuna 2:e84bb87eea71 716
ryuna 0:65b9e62cc2b6 717 Led[1] = Led[2] = Led[3] = 0;
ryuna 0:65b9e62cc2b6 718
ryuna 0:65b9e62cc2b6 719 }
ryuna 0:65b9e62cc2b6 720
ryuna 2:e84bb87eea71 721 Power = 0;
ryuna 2:e84bb87eea71 722 Led[0] = 1;
ryuna 2:e84bb87eea71 723 Degree = 0;
ryuna 2:e84bb87eea71 724 SetC = 0.0;
ryuna 2:e84bb87eea71 725
ryuna 0:65b9e62cc2b6 726
ryuna 0:65b9e62cc2b6 727 Led[3] = 1;
ryuna 5:09afcbe0c18f 728
ryuna 2:e84bb87eea71 729 Degree = IrDegree();
ryuna 0:65b9e62cc2b6 730
ryuna 2:e84bb87eea71 731 if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
ryuna 2:e84bb87eea71 732 (AnotherAction[IrNum/6])();
ryuna 0:65b9e62cc2b6 733 continue;
ryuna 0:65b9e62cc2b6 734 }
ryuna 2:e84bb87eea71 735
ryuna 2:e84bb87eea71 736 /*
ryuna 2:e84bb87eea71 737 if(IrNum == 12){
ryuna 2:e84bb87eea71 738 move(0,0,0,0);
ryuna 2:e84bb87eea71 739 wait_ms(10);
ryuna 2:e84bb87eea71 740 continue;
ryuna 2:e84bb87eea71 741 }*/
ryuna 2:e84bb87eea71 742
ryuna 2:e84bb87eea71 743 nDegree = wrapDegree[Degree/15];
ryuna 6:bfc0e8d2d3ae 744 Power = 35;
ryuna 6:bfc0e8d2d3ae 745
ryuna 6:bfc0e8d2d3ae 746 if((Degree>30)&&(Degree<75)){
ryuna 6:bfc0e8d2d3ae 747 if(IrData[1] < 56){
ryuna 6:bfc0e8d2d3ae 748 if(Degree>180){/*へらす*/
ryuna 6:bfc0e8d2d3ae 749 addDegree = -10;
ryuna 6:bfc0e8d2d3ae 750 }else{
ryuna 6:bfc0e8d2d3ae 751 addDegree = 10;
ryuna 6:bfc0e8d2d3ae 752 }
ryuna 6:bfc0e8d2d3ae 753 }
ryuna 6:bfc0e8d2d3ae 754 }
ryuna 5:09afcbe0c18f 755 Temp = Degree%180;
ryuna 0:65b9e62cc2b6 756
ryuna 6:bfc0e8d2d3ae 757 if((Degree==75)){
ryuna 6:bfc0e8d2d3ae 758 if(IrData[1]<55){
ryuna 6:bfc0e8d2d3ae 759
ryuna 6:bfc0e8d2d3ae 760 }
ryuna 6:bfc0e8d2d3ae 761 }
ryuna 6:bfc0e8d2d3ae 762 if((Temp>80)&&(Temp<100)){
ryuna 5:09afcbe0c18f 763 if(PingData[2]<40){
ryuna 6:bfc0e8d2d3ae 764 if(Degree>180){
ryuna 6:bfc0e8d2d3ae 765 addDegree = 100;
ryuna 6:bfc0e8d2d3ae 766 }else{
ryuna 6:bfc0e8d2d3ae 767 addDegree = -100;
ryuna 6:bfc0e8d2d3ae 768 }
ryuna 6:bfc0e8d2d3ae 769
ryuna 6:bfc0e8d2d3ae 770 }else{
ryuna 6:bfc0e8d2d3ae 771 if(IrData[1] > 65){
ryuna 6:bfc0e8d2d3ae 772 if(Degree>180){
ryuna 6:bfc0e8d2d3ae 773 addDegree = 40;
ryuna 5:09afcbe0c18f 774 }else{
ryuna 6:bfc0e8d2d3ae 775 addDegree = -40;
ryuna 5:09afcbe0c18f 776 }
ryuna 5:09afcbe0c18f 777
ryuna 5:09afcbe0c18f 778 }else {
ryuna 5:09afcbe0c18f 779 addDegree = 0;
ryuna 5:09afcbe0c18f 780
ryuna 5:09afcbe0c18f 781 }
ryuna 5:09afcbe0c18f 782
ryuna 5:09afcbe0c18f 783 }
ryuna 6:bfc0e8d2d3ae 784 }
ryuna 6:bfc0e8d2d3ae 785 /*if(IrData[1] > 65){
ryuna 6:bfc0e8d2d3ae 786 if(Degree>180){
ryuna 6:bfc0e8d2d3ae 787 addDegree = 50;
ryuna 5:09afcbe0c18f 788
ryuna 5:09afcbe0c18f 789 }else{
ryuna 5:09afcbe0c18f 790
ryuna 6:bfc0e8d2d3ae 791 addDegree = -50;
ryuna 5:09afcbe0c18f 792 }
ryuna 5:09afcbe0c18f 793
ryuna 6:bfc0e8d2d3ae 794 }else if(IrData[1] >55){
ryuna 6:bfc0e8d2d3ae 795 if(Degree>180){
ryuna 6:bfc0e8d2d3ae 796 addDegree = 30;
ryuna 5:09afcbe0c18f 797
ryuna 5:09afcbe0c18f 798 }else{
ryuna 6:bfc0e8d2d3ae 799 addDegree = -30;
ryuna 5:09afcbe0c18f 800 }
ryuna 5:09afcbe0c18f 801
ryuna 5:09afcbe0c18f 802
ryuna 5:09afcbe0c18f 803 }else {
ryuna 5:09afcbe0c18f 804 addDegree = 0;
ryuna 5:09afcbe0c18f 805
ryuna 6:bfc0e8d2d3ae 806 }*/
ryuna 6:bfc0e8d2d3ae 807
ryuna 5:09afcbe0c18f 808 if(PingData[2]<40){
ryuna 6:bfc0e8d2d3ae 809 if((Degree>=150)&&(Degree<=300)){
ryuna 6:bfc0e8d2d3ae 810 if(Degree>180){
ryuna 6:bfc0e8d2d3ae 811 addDegree = 90;
ryuna 5:09afcbe0c18f 812
ryuna 5:09afcbe0c18f 813 }else{
ryuna 5:09afcbe0c18f 814
ryuna 6:bfc0e8d2d3ae 815 addDegree = -90;
ryuna 6:bfc0e8d2d3ae 816 }
ryuna 6:bfc0e8d2d3ae 817 }
ryuna 6:bfc0e8d2d3ae 818 }else if(PingData[2]<70){
ryuna 6:bfc0e8d2d3ae 819 if((Degree>=150)&&(Degree<=300)){
ryuna 6:bfc0e8d2d3ae 820 if(Degree>180){
ryuna 6:bfc0e8d2d3ae 821 addDegree = 40;
ryuna 6:bfc0e8d2d3ae 822
ryuna 6:bfc0e8d2d3ae 823 }else{
ryuna 6:bfc0e8d2d3ae 824
ryuna 6:bfc0e8d2d3ae 825 addDegree = -40;
ryuna 6:bfc0e8d2d3ae 826 }
ryuna 6:bfc0e8d2d3ae 827 }
ryuna 6:bfc0e8d2d3ae 828 }
ryuna 6:bfc0e8d2d3ae 829 if(((Degree>=0)&&(Degree<=30))||((Degree>=330)&&(Degree<360))){
ryuna 6:bfc0e8d2d3ae 830 if(IrData[1]<55){
ryuna 6:bfc0e8d2d3ae 831 Power = 15;
ryuna 5:09afcbe0c18f 832 }
ryuna 5:09afcbe0c18f 833 }
ryuna 6:bfc0e8d2d3ae 834
ryuna 6:bfc0e8d2d3ae 835 if(((Degree>30)&&(Degree<75))||((Degree>315)&&(Degree<330))){
ryuna 6:bfc0e8d2d3ae 836 if(IrData[1]<55){
ryuna 6:bfc0e8d2d3ae 837 Power = 32;
ryuna 6:bfc0e8d2d3ae 838 }
ryuna 6:bfc0e8d2d3ae 839 }
ryuna 5:09afcbe0c18f 840 Degree = nDegree + addDegree;
ryuna 0:65b9e62cc2b6 841
ryuna 2:e84bb87eea71 842 if((Degree <0)||(Degree>=360)){
ryuna 2:e84bb87eea71 843 Degree = 0;
ryuna 2:e84bb87eea71 844 }
ryuna 6:bfc0e8d2d3ae 845 //Lcd.printf("%d\n",Degree);
ryuna 2:e84bb87eea71 846 fool(&Degree,&Power);
ryuna 6:bfc0e8d2d3ae 847 move(Power,Power,0,Degree);
ryuna 2:e84bb87eea71 848
ryuna 2:e84bb87eea71 849 //wait_ms(500);
ryuna 6:bfc0e8d2d3ae 850 //Lcd.cls();
ryuna 2:e84bb87eea71 851 Led[0] =0;
ryuna 0:65b9e62cc2b6 852 wait_ms(10);
ryuna 0:65b9e62cc2b6 853
ryuna 0:65b9e62cc2b6 854 }
ryuna 5:09afcbe0c18f 855 int i;
ryuna 5:09afcbe0c18f 856 while(1){
ryuna 0:65b9e62cc2b6 857 //デモプログラム
ryuna 3:4a39486ff238 858 //Receive();
ryuna 5:09afcbe0c18f 859 for(i = 0; i<360; i++){
ryuna 5:09afcbe0c18f 860 Power = 20;
ryuna 5:09afcbe0c18f 861 fool(&i,&Power);
ryuna 5:09afcbe0c18f 862
ryuna 5:09afcbe0c18f 863 move(Power,Power,0,i);
ryuna 5:09afcbe0c18f 864 wait_ms(200);
ryuna 5:09afcbe0c18f 865 }
ryuna 5:09afcbe0c18f 866 //pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
ryuna 4:2857f273a7f4 867 //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
ryuna 4:2857f273a7f4 868
ryuna 3:4a39486ff238 869 //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
ryuna 4:2857f273a7f4 870 //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
ryuna 2:e84bb87eea71 871 wait(0.1);
ryuna 0:65b9e62cc2b6 872 }
ryuna 0:65b9e62cc2b6 873
ryuna 1:f91d53098d57 874
ryuna 0:65b9e62cc2b6 875
ryuna 0:65b9e62cc2b6 876
ryuna 0:65b9e62cc2b6 877 }