CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

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

Dependencies:   AQM0802A PID Servo mbed

main.cpp@5:09afcbe0c18f, 2015-03-13 (annotated)

Committer:
ryuna
Date:
Fri Mar 13 01:14:38 2015 +0000
Revision:
5:09afcbe0c18f
Parent:
4:2857f273a7f4
Child:
6:bfc0e8d2d3ae
Child:
7:01bb0b0a98f1
??; ; ??????;

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 3:4a39486ff238 53
ryuna 0:65b9e62cc2b6 54
ryuna 2:e84bb87eea71 55 void move(int vr,int vl, double vs ,int Degree){
ryuna 0:65b9e62cc2b6 56 double pwm[4] = {0};
ryuna 0:65b9e62cc2b6 57 uint8_t i = 0;
ryuna 5:09afcbe0c18f 58 static int last_deg;
ryuna 5:09afcbe0c18f 59 double vias=30.0;
ryuna 5:09afcbe0c18f 60 double dif;
ryuna 5:09afcbe0c18f 61 if(abs(last_deg)<23){
ryuna 5:09afcbe0c18f 62
ryuna 5:09afcbe0c18f 63 dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
ryuna 5:09afcbe0c18f 64 if(abs(dif)<vias){
ryuna 5:09afcbe0c18f 65
ryuna 5:09afcbe0c18f 66 pwm[0] = vr;
ryuna 5:09afcbe0c18f 67 pwm[1] = 0;
ryuna 5:09afcbe0c18f 68 pwm[2] = 0;
ryuna 5:09afcbe0c18f 69 pwm[3] = vl;
ryuna 5:09afcbe0c18f 70 }
ryuna 5:09afcbe0c18f 71 else{
ryuna 5:09afcbe0c18f 72
ryuna 5:09afcbe0c18f 73 if(dif<0){
ryuna 5:09afcbe0c18f 74 //vs = vs - (20-vias);
ryuna 5:09afcbe0c18f 75 vs = -(20+(abs(dif))*0.1);
ryuna 5:09afcbe0c18f 76 if( abs(dif) >= 100) vs = -(20+(100)*0.1);
ryuna 5:09afcbe0c18f 77 }
ryuna 5:09afcbe0c18f 78 else{
ryuna 5:09afcbe0c18f 79 //vs = vs + (20-vias);
ryuna 5:09afcbe0c18f 80 vs = (20+(abs(dif))*0.1);
ryuna 5:09afcbe0c18f 81 if( abs(dif) >= 100) vs = (20+(100)*0.1);
ryuna 5:09afcbe0c18f 82 }
ryuna 5:09afcbe0c18f 83
ryuna 5:09afcbe0c18f 84 pwm[0] = vs;
ryuna 5:09afcbe0c18f 85 pwm[1] = 0;
ryuna 5:09afcbe0c18f 86 pwm[2] = 0;
ryuna 5:09afcbe0c18f 87 pwm[3] = -vs;
ryuna 5:09afcbe0c18f 88 }
ryuna 5:09afcbe0c18f 89 }else{
ryuna 5:09afcbe0c18f 90 pwm[0] = vr;
ryuna 4:2857f273a7f4 91 pwm[1] = 0;
ryuna 4:2857f273a7f4 92 pwm[2] = 0;
ryuna 5:09afcbe0c18f 93 pwm[3] = vl;
ryuna 4:2857f273a7f4 94 }
ryuna 5:09afcbe0c18f 95
ryuna 5:09afcbe0c18f 96
ryuna 5:09afcbe0c18f 97 if(!WaitFlag){
ryuna 5:09afcbe0c18f 98 SetDegree = Degree * 11/9;
ryuna 5:09afcbe0c18f 99 if(SetDegree > 110){
ryuna 5:09afcbe0c18f 100 SetDegree = 110;
ryuna 5:09afcbe0c18f 101 }else if(SetDegree < -110){
ryuna 5:09afcbe0c18f 102 SetDegree = -110;
ryuna 5:09afcbe0c18f 103 }
ryuna 5:09afcbe0c18f 104 S555.position(SetDegree);
ryuna 5:09afcbe0c18f 105 WaitFlag = 1;
ryuna 5:09afcbe0c18f 106 STimer.attach(&ServoWait,WAIT_TIME);
ryuna 5:09afcbe0c18f 107
ryuna 5:09afcbe0c18f 108 last_deg = Degree;
ryuna 5:09afcbe0c18f 109 }
ryuna 5:09afcbe0c18f 110
ryuna 5:09afcbe0c18f 111
ryuna 0:65b9e62cc2b6 112 for(i = 0; i < 4; i++){
ryuna 0:65b9e62cc2b6 113 if(pwm[i] > 100){
ryuna 0:65b9e62cc2b6 114 pwm[i] = 100;
ryuna 0:65b9e62cc2b6 115 }else if(pwm[i] < -100){
ryuna 0:65b9e62cc2b6 116 pwm[i] = -100;
ryuna 0:65b9e62cc2b6 117 }
ryuna 0:65b9e62cc2b6 118 speed[i] = pwm[i];
ryuna 0:65b9e62cc2b6 119 }
ryuna 5:09afcbe0c18f 120
ryuna 5:09afcbe0c18f 121
ryuna 0:65b9e62cc2b6 122 wait_ms(10);
ryuna 0:65b9e62cc2b6 123 }
ryuna 2:e84bb87eea71 124
ryuna 2:e84bb87eea71 125 uint8_t PingChange(uint8_t LineData){
ryuna 2:e84bb87eea71 126 static uint8_t Last_Line;
ryuna 2:e84bb87eea71 127 static uint8_t Last_Ping;
ryuna 2:e84bb87eea71 128 uint8_t LinePing = 0;
ryuna 2:e84bb87eea71 129
ryuna 2:e84bb87eea71 130 if(!LineData){
ryuna 2:e84bb87eea71 131 return 0;
ryuna 2:e84bb87eea71 132 }
ryuna 2:e84bb87eea71 133
ryuna 5:09afcbe0c18f 134 if(PingData[0] <50) LinePing = LinePing + 1;
ryuna 5:09afcbe0c18f 135 if(PingData[1] <50) LinePing = LinePing + 2;
ryuna 5:09afcbe0c18f 136 if(PingData[2] <50) LinePing = LinePing + 4;
ryuna 5:09afcbe0c18f 137 if(PingData[3] <50) LinePing = LinePing + 8;
ryuna 2:e84bb87eea71 138
ryuna 2:e84bb87eea71 139
ryuna 2:e84bb87eea71 140 if(LinePing&0x01){
ryuna 2:e84bb87eea71 141 if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
ryuna 2:e84bb87eea71 142 Last_Ping = LinePing;
ryuna 2:e84bb87eea71 143 Last_Line = LineData;
ryuna 2:e84bb87eea71 144 return 1;
ryuna 2:e84bb87eea71 145 }
ryuna 2:e84bb87eea71 146 }
ryuna 2:e84bb87eea71 147 if(LinePing&0x02){
ryuna 2:e84bb87eea71 148 if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
ryuna 2:e84bb87eea71 149 Last_Ping = LinePing;
ryuna 2:e84bb87eea71 150 Last_Line = LineData;
ryuna 2:e84bb87eea71 151 return 2;
ryuna 2:e84bb87eea71 152 }
ryuna 2:e84bb87eea71 153 }
ryuna 2:e84bb87eea71 154 if(LinePing&0x04){
ryuna 2:e84bb87eea71 155 if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
ryuna 2:e84bb87eea71 156 Last_Ping = LinePing;
ryuna 2:e84bb87eea71 157 Last_Line = LineData;
ryuna 2:e84bb87eea71 158 return 4;
ryuna 2:e84bb87eea71 159 }
ryuna 2:e84bb87eea71 160 }
ryuna 2:e84bb87eea71 161 if(LinePing&0x08){
ryuna 2:e84bb87eea71 162 if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
ryuna 2:e84bb87eea71 163 Last_Ping = LinePing;
ryuna 2:e84bb87eea71 164 Last_Line = LineData;
ryuna 2:e84bb87eea71 165 return 8;
ryuna 2:e84bb87eea71 166 }
ryuna 2:e84bb87eea71 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 3:4a39486ff238 294 if(IrData[1]>70){
ryuna 2:e84bb87eea71 295 move(30,30,CompassPID,0);
ryuna 2:e84bb87eea71 296 return;
ryuna 2:e84bb87eea71 297 }
ryuna 3:4a39486ff238 298 if(IrData[1]>60){
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 2:e84bb87eea71 304 if(PingData[0]<50){
ryuna 2:e84bb87eea71 305 if((PingData[1]<60)&&(PingData[1]>40)){
ryuna 2:e84bb87eea71 306 /*右側に居る*/
ryuna 5:09afcbe0c18f 307 move(40,25,0,10);
ryuna 5:09afcbe0c18f 308 Kick = 1;
ryuna 5:09afcbe0c18f 309 wait_ms(200);
ryuna 2:e84bb87eea71 310 move(30,30,0,0);
ryuna 5:09afcbe0c18f 311 Kick = 0;
ryuna 2:e84bb87eea71 312 return;
ryuna 2:e84bb87eea71 313 }
ryuna 2:e84bb87eea71 314 if((PingData[1]<60)&&(PingData[1]>40)){
ryuna 2:e84bb87eea71 315 /*左側に居る*/
ryuna 5:09afcbe0c18f 316 move(25,40,0,-10);
ryuna 5:09afcbe0c18f 317 Kick = 1;
ryuna 5:09afcbe0c18f 318 wait_ms(200);
ryuna 2:e84bb87eea71 319 move(30,30,0,0);
ryuna 5:09afcbe0c18f 320 Kick = 0;
ryuna 2:e84bb87eea71 321 return;
ryuna 2:e84bb87eea71 322 }
ryuna 2:e84bb87eea71 323 /*それ以外*/
ryuna 5:09afcbe0c18f 324
ryuna 4:2857f273a7f4 325 //Receive();
ryuna 2:e84bb87eea71 326 if(!(IrNum == 0)) return;
ryuna 2:e84bb87eea71 327 move(40,40,CompassPID,0);
ryuna 5:09afcbe0c18f 328 Kick = 1;
ryuna 5:09afcbe0c18f 329 wait_ms(100);
ryuna 5:09afcbe0c18f 330 Kick = 0;
ryuna 2:e84bb87eea71 331 return ;
ryuna 2:e84bb87eea71 332 }
ryuna 2:e84bb87eea71 333 move(20,20,CompassPID,0);
ryuna 2:e84bb87eea71 334
ryuna 2:e84bb87eea71 335
ryuna 0:65b9e62cc2b6 336
ryuna 0:65b9e62cc2b6 337 }
ryuna 0:65b9e62cc2b6 338
ryuna 2:e84bb87eea71 339 void IrBackAction()//ball found six o-clock
ryuna 0:65b9e62cc2b6 340 {
ryuna 0:65b9e62cc2b6 341 /***
ryuna 0:65b9e62cc2b6 342 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
ryuna 0:65b9e62cc2b6 343 *
ryuna 0:65b9e62cc2b6 344 **/
ryuna 2:e84bb87eea71 345 if(PingData[1]>PingData[3]){
ryuna 0:65b9e62cc2b6 346 /*右が大きい*/
ryuna 3:4a39486ff238 347 if(IrData[1]>70){
ryuna 2:e84bb87eea71 348 move(-20,-20,CompassPID,45);
ryuna 2:e84bb87eea71 349 return;
ryuna 2:e84bb87eea71 350 }
ryuna 3:4a39486ff238 351 if(IrData[1]>60){
ryuna 2:e84bb87eea71 352 move(-20,-20,CompassPID,60);
ryuna 2:e84bb87eea71 353 return;
ryuna 2:e84bb87eea71 354 }
ryuna 3:4a39486ff238 355 move(-20,-20,CompassPID,80);
ryuna 0:65b9e62cc2b6 356 return;
ryuna 0:65b9e62cc2b6 357 }
ryuna 0:65b9e62cc2b6 358 /*左が大きい*/
ryuna 0:65b9e62cc2b6 359
ryuna 3:4a39486ff238 360 if(IrData[1]>70){
ryuna 2:e84bb87eea71 361 move(-20,-20,CompassPID,-45);
ryuna 2:e84bb87eea71 362 return;
ryuna 0:65b9e62cc2b6 363 }
ryuna 3:4a39486ff238 364 if(IrData[1]>60){
ryuna 2:e84bb87eea71 365 move(-20,-20,CompassPID,-60);
ryuna 2:e84bb87eea71 366 return;
ryuna 2:e84bb87eea71 367 }
ryuna 3:4a39486ff238 368 move(-20,-20,CompassPID,-80);
ryuna 2:e84bb87eea71 369 return;
ryuna 0:65b9e62cc2b6 370
ryuna 2:e84bb87eea71 371 }
ryuna 2:e84bb87eea71 372
ryuna 2:e84bb87eea71 373 void GoHome()//Ball is not found.
ryuna 2:e84bb87eea71 374 {
ryuna 5:09afcbe0c18f 375 int pingRl =0;
ryuna 5:09afcbe0c18f 376 int PowLev;
ryuna 5:09afcbe0c18f 377 if(PingData[2] >=50){//後ろからの距離60cm
ryuna 5:09afcbe0c18f 378 move(-25,-25,CompassPID,0);
ryuna 5:09afcbe0c18f 379 return ;
ryuna 5:09afcbe0c18f 380 }
ryuna 5:09afcbe0c18f 381 if(PingData[2] <= 30){
ryuna 5:09afcbe0c18f 382 move(17,17,0,0);
ryuna 0:65b9e62cc2b6 383 return ;
ryuna 5:09afcbe0c18f 384 }
ryuna 5:09afcbe0c18f 385 pingRl = PingData[1]+PingData[3];
ryuna 5:09afcbe0c18f 386 if(pingRl <100){
ryuna 5:09afcbe0c18f 387 move(0,0,CompassPID,0);//stop
ryuna 5:09afcbe0c18f 388 return;
ryuna 5:09afcbe0c18f 389 }
ryuna 5:09afcbe0c18f 390 if(PingData[1] < 52){
ryuna 5:09afcbe0c18f 391 PowLev = (52 - PingData[1])*1.3;
ryuna 5:09afcbe0c18f 392 move(PowLev,PowLev,0,90);
ryuna 5:09afcbe0c18f 393 return ;
ryuna 5:09afcbe0c18f 394 }else if (PingData[3] < 52){
ryuna 5:09afcbe0c18f 395 PowLev = (52 - PingData[3])*1.35;
ryuna 5:09afcbe0c18f 396 move(PowLev,PowLev,0,-90);
ryuna 5:09afcbe0c18f 397 return ;
ryuna 5:09afcbe0c18f 398 }
ryuna 5:09afcbe0c18f 399
ryuna 5:09afcbe0c18f 400 move(0,0,0,0);//stop
ryuna 2:e84bb87eea71 401
ryuna 0:65b9e62cc2b6 402 }
ryuna 0:65b9e62cc2b6 403
ryuna 0:65b9e62cc2b6 404
ryuna 0:65b9e62cc2b6 405 void PidUpdate()
ryuna 0:65b9e62cc2b6 406 {
ryuna 5:09afcbe0c18f 407
ryuna 5:09afcbe0c18f 408 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 5:09afcbe0c18f 409 InputPID = ((Compass/10) + 540 - CompassDef) % 360;
ryuna 5:09afcbe0c18f 410
ryuna 5:09afcbe0c18f 411 pid.setProcessValue(InputPID);
ryuna 5:09afcbe0c18f 412 CompassPID = (pid.compute());
ryuna 5:09afcbe0c18f 413 /*
ryuna 2:e84bb87eea71 414 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 2:e84bb87eea71 415 InputPID = Compass;
ryuna 2:e84bb87eea71 416
ryuna 2:e84bb87eea71 417 pid.setProcessValue(InputPID);
ryuna 4:2857f273a7f4 418 CompassPID = (pid.compute());
ryuna 5:09afcbe0c18f 419 */
ryuna 0:65b9e62cc2b6 420 }
ryuna 0:65b9e62cc2b6 421
ryuna 0:65b9e62cc2b6 422
ryuna 0:65b9e62cc2b6 423
ryuna 0:65b9e62cc2b6 424
ryuna 0:65b9e62cc2b6 425
ryuna 0:65b9e62cc2b6 426
ryuna 0:65b9e62cc2b6 427
ryuna 0:65b9e62cc2b6 428 uint8_t SwRead(){
ryuna 0:65b9e62cc2b6 429 /******
ryuna 0:65b9e62cc2b6 430 *retrun : sw_state
ryuna 0:65b9e62cc2b6 431 *StartS = 0x01;
ryuna 0:65b9e62cc2b6 432 *Debug2 = 0x02;
ryuna 0:65b9e62cc2b6 433 *Debug1 = 0x04;
ryuna 0:65b9e62cc2b6 434 *Debug3 = 0x06;
ryuna 0:65b9e62cc2b6 435 *Kicker = 0x08;
ryuna 0:65b9e62cc2b6 436 *Calibration = 0x10;
ryuna 0:65b9e62cc2b6 437
ryuna 0:65b9e62cc2b6 438 *
ryuna 0:65b9e62cc2b6 439 *****/
ryuna 0:65b9e62cc2b6 440 uint8_t i,temp,temp2;
ryuna 0:65b9e62cc2b6 441 temp = ~Sw - 224;
ryuna 0:65b9e62cc2b6 442 if(!(temp == Kicker
ryuna 0:65b9e62cc2b6 443 ||temp == Debug1
ryuna 0:65b9e62cc2b6 444 ||temp == Debug2
ryuna 0:65b9e62cc2b6 445 ||temp == Debug3
ryuna 5:09afcbe0c18f 446 ||temp == Debug12
ryuna 5:09afcbe0c18f 447 ||temp == Debug23
ryuna 0:65b9e62cc2b6 448 ||temp == StartS)) return 0;/*スイッチが押されていない*/
ryuna 0:65b9e62cc2b6 449 if(!(temp == 0x00)){
ryuna 0:65b9e62cc2b6 450 for(i = 0; i < 50; i++);
ryuna 0:65b9e62cc2b6 451 temp2 = ~Sw - 224;
ryuna 0:65b9e62cc2b6 452 if(temp == temp2){
ryuna 0:65b9e62cc2b6 453 return temp;
ryuna 0:65b9e62cc2b6 454 }
ryuna 0:65b9e62cc2b6 455 }
ryuna 0:65b9e62cc2b6 456 return 0;
ryuna 0:65b9e62cc2b6 457 }
ryuna 0:65b9e62cc2b6 458
ryuna 0:65b9e62cc2b6 459 //通信（モータ用）
ryuna 0:65b9e62cc2b6 460 void tx_motor(){
ryuna 0:65b9e62cc2b6 461 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:65b9e62cc2b6 462 Motor.printf("%s",StringFIN.c_str());
ryuna 0:65b9e62cc2b6 463 }
ryuna 0:65b9e62cc2b6 464
ryuna 0:65b9e62cc2b6 465 void SetUp(){
ryuna 0:65b9e62cc2b6 466 /*初期化*/
ryuna 3:4a39486ff238 467
ryuna 0:65b9e62cc2b6 468 Motor.baud(115200); //ボーレート設定
ryuna 0:65b9e62cc2b6 469 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:65b9e62cc2b6 470 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:65b9e62cc2b6 471
ryuna 4:2857f273a7f4 472 Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
ryuna 4:2857f273a7f4 473
ryuna 5:09afcbe0c18f 474 S555.calibrate(0.0006, 120.0);
ryuna 2:e84bb87eea71 475 //S555.position(0.0); //初期位置にセット
ryuna 0:65b9e62cc2b6 476 move(0,0,0,0);//停止
ryuna 0:65b9e62cc2b6 477
ryuna 5:09afcbe0c18f 478
ryuna 0:65b9e62cc2b6 479 Kick = 0;
ryuna 5:09afcbe0c18f 480
ryuna 0:65b9e62cc2b6 481 Sw.mode(PullUp);
ryuna 0:65b9e62cc2b6 482
ryuna 0:65b9e62cc2b6 483 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
ryuna 0:65b9e62cc2b6 484 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
ryuna 0:65b9e62cc2b6 485 pid.setBias(PID_BIAS); //pid sed def
ryuna 0:65b9e62cc2b6 486 pid.setMode(AUTO_MODE); //pid sed def
ryuna 0:65b9e62cc2b6 487 pid.setSetPoint(REFERENCE); //pid sed def
ryuna 1:f91d53098d57 488
ryuna 0:65b9e62cc2b6 489
ryuna 5:09afcbe0c18f 490 for(int i=0; i<15; i++){
ryuna 5:09afcbe0c18f 491 CompassDef = Compass/10;
ryuna 5:09afcbe0c18f 492 wait_ms(10);
ryuna 5:09afcbe0c18f 493 }
ryuna 5:09afcbe0c18f 494 //pidupdate.attach(&PidUpdate, PID_CYCLE);
ryuna 5:09afcbe0c18f 495
ryuna 0:65b9e62cc2b6 496 }
ryuna 0:65b9e62cc2b6 497 void StartLoop(){
ryuna 0:65b9e62cc2b6 498
ryuna 0:65b9e62cc2b6 499 uint8_t State = 0;
ryuna 0:65b9e62cc2b6 500 uint8_t LineData = 0;
ryuna 5:09afcbe0c18f 501 int a;
ryuna 0:65b9e62cc2b6 502 while(1){
ryuna 0:65b9e62cc2b6 503 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 0:65b9e62cc2b6 504 //Lcd.cls();
ryuna 2:e84bb87eea71 505
ryuna 0:65b9e62cc2b6 506 State = SwRead();
ryuna 0:65b9e62cc2b6 507 if(State == 0) continue;
ryuna 0:65b9e62cc2b6 508
ryuna 0:65b9e62cc2b6 509 if(State == StartS){
ryuna 0:65b9e62cc2b6 510 /*loop end & start*/
ryuna 0:65b9e62cc2b6 511 return;
ryuna 0:65b9e62cc2b6 512 }
ryuna 0:65b9e62cc2b6 513
ryuna 0:65b9e62cc2b6 514 if(State == Debug1){
ryuna 0:65b9e62cc2b6 515 while((State == Debug1)){
ryuna 0:65b9e62cc2b6 516 LineData = (~Line+0x00) & 0x0F;
ryuna 0:65b9e62cc2b6 517 Lcd.printf("%d\n",LineData);
ryuna 0:65b9e62cc2b6 518
ryuna 0:65b9e62cc2b6 519 wait_ms(100);
ryuna 0:65b9e62cc2b6 520 State = SwRead();
ryuna 0:65b9e62cc2b6 521 }
ryuna 0:65b9e62cc2b6 522 Lcd.cls();
ryuna 0:65b9e62cc2b6 523 continue;
ryuna 0:65b9e62cc2b6 524
ryuna 0:65b9e62cc2b6 525 }
ryuna 0:65b9e62cc2b6 526 if(State == Debug2){
ryuna 0:65b9e62cc2b6 527 while((State == Debug2)){
ryuna 4:2857f273a7f4 528 //Receive();
ryuna 2:e84bb87eea71 529 Lcd.printf("%d\n",IrNum);
ryuna 0:65b9e62cc2b6 530 wait_ms(100);
ryuna 0:65b9e62cc2b6 531 State = SwRead();
ryuna 0:65b9e62cc2b6 532 }
ryuna 0:65b9e62cc2b6 533 Lcd.cls();
ryuna 0:65b9e62cc2b6 534 continue;
ryuna 0:65b9e62cc2b6 535 }
ryuna 0:65b9e62cc2b6 536
ryuna 0:65b9e62cc2b6 537 if(State == Debug3){
ryuna 0:65b9e62cc2b6 538 while((State == Debug3)){
ryuna 5:09afcbe0c18f 539 a = (Compass/10 + 540 - CompassDef) % 360;
ryuna 5:09afcbe0c18f 540 Lcd.printf("%d\n",a);
ryuna 0:65b9e62cc2b6 541 wait_ms(100);
ryuna 0:65b9e62cc2b6 542 State = SwRead();
ryuna 0:65b9e62cc2b6 543 }
ryuna 0:65b9e62cc2b6 544 Lcd.cls();
ryuna 0:65b9e62cc2b6 545 continue;
ryuna 0:65b9e62cc2b6 546 }
ryuna 5:09afcbe0c18f 547 if(State == Debug12){
ryuna 5:09afcbe0c18f 548 while((State == Debug12)){
ryuna 5:09afcbe0c18f 549 Lcd.printf("%d %d\n%d %d\n",PingData[0],PingData[1],PingData[2],PingData[3]);
ryuna 5:09afcbe0c18f 550 wait_ms(100);
ryuna 5:09afcbe0c18f 551 State = SwRead();
ryuna 5:09afcbe0c18f 552 }
ryuna 5:09afcbe0c18f 553 Lcd.cls();
ryuna 5:09afcbe0c18f 554 continue;
ryuna 5:09afcbe0c18f 555 }
ryuna 0:65b9e62cc2b6 556 if(State == Kicker){
ryuna 0:65b9e62cc2b6 557 Led[0] = Led[1] = Led[2] = 0;
ryuna 4:2857f273a7f4 558 Kick = 1;
ryuna 4:2857f273a7f4 559 wait_ms(500);
ryuna 4:2857f273a7f4 560 Kick = 0;
ryuna 0:65b9e62cc2b6 561 while((State == Kicker)){
ryuna 0:65b9e62cc2b6 562 wait_ms(100);
ryuna 0:65b9e62cc2b6 563 State = SwRead();
ryuna 0:65b9e62cc2b6 564 }
ryuna 0:65b9e62cc2b6 565 continue;
ryuna 0:65b9e62cc2b6 566 }
ryuna 0:65b9e62cc2b6 567 }
ryuna 0:65b9e62cc2b6 568
ryuna 0:65b9e62cc2b6 569 }
ryuna 0:65b9e62cc2b6 570 int main() {
ryuna 0:65b9e62cc2b6 571
ryuna 0:65b9e62cc2b6 572 //uint8_t IrNumOld = 0;//過去値
ryuna 0:65b9e62cc2b6 573 /*Line*/
ryuna 0:65b9e62cc2b6 574 uint8_t LineData = 0;
ryuna 2:e84bb87eea71 575 uint8_t LinePing = 0;
ryuna 0:65b9e62cc2b6 576
ryuna 2:e84bb87eea71 577
ryuna 0:65b9e62cc2b6 578 /*State */
ryuna 0:65b9e62cc2b6 579 uint8_t LineIr = 0;
ryuna 5:09afcbe0c18f 580 uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
ryuna 3:4a39486ff238 581
ryuna 5:09afcbe0c18f 582 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
ryuna 2:e84bb87eea71 583
ryuna 0:65b9e62cc2b6 584 /*行動設定*/
ryuna 0:65b9e62cc2b6 585 int Power = 0;
ryuna 2:e84bb87eea71 586 int Degree = 0;
ryuna 0:65b9e62cc2b6 587
ryuna 0:65b9e62cc2b6 588 /*楽しい変数達*/
ryuna 2:e84bb87eea71 589 int nDegree =0;//基礎角
ryuna 2:e84bb87eea71 590 int addDegree = 0;//追加角
ryuna 5:09afcbe0c18f 591 int Temp = 0;
ryuna 0:65b9e62cc2b6 592
ryuna 5:09afcbe0c18f 593 int StopCt = 0;
ryuna 0:65b9e62cc2b6 594
ryuna 0:65b9e62cc2b6 595 /*関数ポインタ*/
ryuna 2:e84bb87eea71 596
ryuna 2:e84bb87eea71 597
ryuna 2:e84bb87eea71 598 //void (*AnotherAction[3])(uint8_t [],double);
ryuna 2:e84bb87eea71 599 void (*AnotherAction[3])();
ryuna 0:65b9e62cc2b6 600 AnotherAction[0] = IrFrontAction;
ryuna 0:65b9e62cc2b6 601 AnotherAction[1] = IrBackAction;
ryuna 0:65b9e62cc2b6 602 AnotherAction[2] = GoHome;
ryuna 1:f91d53098d57 603
ryuna 2:e84bb87eea71 604
ryuna 0:65b9e62cc2b6 605 SetUp();/*set up routine*/
ryuna 4:2857f273a7f4 606
ryuna 0:65b9e62cc2b6 607 StartLoop(); /*loop strat, switch push end*/
ryuna 2:e84bb87eea71 608 Led[0] = Led[1] = Led[2] = Led[3] = 0;
ryuna 2:e84bb87eea71 609 wait_ms(100);
ryuna 0:65b9e62cc2b6 610
ryuna 4:2857f273a7f4 611 while(1){
ryuna 0:65b9e62cc2b6 612
ryuna 5:09afcbe0c18f 613 //S555.calibrate(0.0006, 120.0);
ryuna 2:e84bb87eea71 614
ryuna 4:2857f273a7f4 615 //Receive();
ryuna 2:e84bb87eea71 616 //Lcd.printf("%d\n",IrNum);
ryuna 0:65b9e62cc2b6 617 /*白線を読んでいないか確認する*/
ryuna 5:09afcbe0c18f 618
ryuna 0:65b9e62cc2b6 619 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 620
ryuna 0:65b9e62cc2b6 621 if(LineData){
ryuna 2:e84bb87eea71 622 LineIr = LineData & IrChange[IrNum];
ryuna 2:e84bb87eea71 623 LinePing = PingChange(LineData);
ryuna 5:09afcbe0c18f 624 if(LinePing){
ryuna 5:09afcbe0c18f 625 move(0,0,0,0);
ryuna 5:09afcbe0c18f 626 wait_ms(200);
ryuna 5:09afcbe0c18f 627 }
ryuna 2:e84bb87eea71 628 if(LineIr){
ryuna 2:e84bb87eea71 629 move(0,0,0,0);
ryuna 5:09afcbe0c18f 630 StopCt = 0;
ryuna 2:e84bb87eea71 631 while(LineIr){
ryuna 5:09afcbe0c18f 632 if(StopCt>30){
ryuna 5:09afcbe0c18f 633 GoHome();
ryuna 5:09afcbe0c18f 634 wait_ms(100);
ryuna 5:09afcbe0c18f 635 }
ryuna 5:09afcbe0c18f 636 StopCt++;
ryuna 2:e84bb87eea71 637 Led[1] = Led[2] = Led[3] = 1;
ryuna 4:2857f273a7f4 638 //Receive();
ryuna 2:e84bb87eea71 639 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 640 LineIr = LineData & IrChange[IrNum];
ryuna 2:e84bb87eea71 641 wait_ms(10);
ryuna 2:e84bb87eea71 642 }
ryuna 2:e84bb87eea71 643 }else if(LinePing){
ryuna 3:4a39486ff238 644 move(0,0,0,0);
ryuna 5:09afcbe0c18f 645 StopCt = 0;
ryuna 2:e84bb87eea71 646 while(LinePing){
ryuna 5:09afcbe0c18f 647 if(StopCt>30){
ryuna 5:09afcbe0c18f 648 GoHome();
ryuna 5:09afcbe0c18f 649 wait_ms(200);
ryuna 5:09afcbe0c18f 650 }
ryuna 5:09afcbe0c18f 651 StopCt++;
ryuna 2:e84bb87eea71 652 Led[1] = Led[2] = Led[3] = 1;
ryuna 4:2857f273a7f4 653 //Receive();
ryuna 2:e84bb87eea71 654 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 655 LinePing = PingChange(LineData);
ryuna 3:4a39486ff238 656
ryuna 2:e84bb87eea71 657 wait_ms(10);
ryuna 2:e84bb87eea71 658 }
ryuna 0:65b9e62cc2b6 659 }
ryuna 2:e84bb87eea71 660
ryuna 2:e84bb87eea71 661
ryuna 0:65b9e62cc2b6 662 Led[1] = Led[2] = Led[3] = 0;
ryuna 0:65b9e62cc2b6 663
ryuna 0:65b9e62cc2b6 664 }
ryuna 0:65b9e62cc2b6 665
ryuna 2:e84bb87eea71 666 Power = 0;
ryuna 2:e84bb87eea71 667 Led[0] = 1;
ryuna 2:e84bb87eea71 668 Degree = 0;
ryuna 2:e84bb87eea71 669 SetC = 0.0;
ryuna 2:e84bb87eea71 670
ryuna 0:65b9e62cc2b6 671
ryuna 0:65b9e62cc2b6 672 Led[3] = 1;
ryuna 5:09afcbe0c18f 673
ryuna 2:e84bb87eea71 674 Degree = IrDegree();
ryuna 0:65b9e62cc2b6 675
ryuna 2:e84bb87eea71 676 if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
ryuna 2:e84bb87eea71 677 (AnotherAction[IrNum/6])();
ryuna 0:65b9e62cc2b6 678 continue;
ryuna 0:65b9e62cc2b6 679 }
ryuna 2:e84bb87eea71 680
ryuna 2:e84bb87eea71 681 /*
ryuna 2:e84bb87eea71 682 if(IrNum == 12){
ryuna 2:e84bb87eea71 683 move(0,0,0,0);
ryuna 2:e84bb87eea71 684 wait_ms(10);
ryuna 2:e84bb87eea71 685 continue;
ryuna 2:e84bb87eea71 686 }*/
ryuna 2:e84bb87eea71 687
ryuna 2:e84bb87eea71 688 nDegree = wrapDegree[Degree/15];
ryuna 5:09afcbe0c18f 689 Power = 30;
ryuna 5:09afcbe0c18f 690 Temp = Degree%180;
ryuna 0:65b9e62cc2b6 691
ryuna 5:09afcbe0c18f 692 if((Temp>70)&&(Temp<110)){
ryuna 5:09afcbe0c18f 693 if(PingData[2]<40){
ryuna 5:09afcbe0c18f 694 if(IrData[1] > 70){
ryuna 5:09afcbe0c18f 695 if(Degree/180){
ryuna 5:09afcbe0c18f 696 addDegree = 90;
ryuna 5:09afcbe0c18f 697 }else{
ryuna 5:09afcbe0c18f 698 addDegree = -90;
ryuna 5:09afcbe0c18f 699 }
ryuna 5:09afcbe0c18f 700 }else if(IrData[1] >60){
ryuna 5:09afcbe0c18f 701 if(Degree/180){
ryuna 5:09afcbe0c18f 702 addDegree = 60;
ryuna 5:09afcbe0c18f 703 }else{
ryuna 5:09afcbe0c18f 704 addDegree = -60;
ryuna 5:09afcbe0c18f 705 }
ryuna 5:09afcbe0c18f 706 }else{
ryuna 5:09afcbe0c18f 707 addDegree = 0;
ryuna 5:09afcbe0c18f 708 }
ryuna 5:09afcbe0c18f 709 }else{
ryuna 5:09afcbe0c18f 710 if(IrData[1] > 70){
ryuna 5:09afcbe0c18f 711 if(Degree/180){
ryuna 5:09afcbe0c18f 712 addDegree = 30;
ryuna 5:09afcbe0c18f 713
ryuna 5:09afcbe0c18f 714 }else{
ryuna 5:09afcbe0c18f 715
ryuna 5:09afcbe0c18f 716 addDegree = -30;
ryuna 5:09afcbe0c18f 717 }
ryuna 5:09afcbe0c18f 718
ryuna 5:09afcbe0c18f 719 }else if(IrData[1] >60){
ryuna 5:09afcbe0c18f 720 if(Degree/180){
ryuna 5:09afcbe0c18f 721 addDegree = 20;
ryuna 5:09afcbe0c18f 722
ryuna 5:09afcbe0c18f 723 }else{
ryuna 5:09afcbe0c18f 724 addDegree = -20;
ryuna 5:09afcbe0c18f 725 }
ryuna 5:09afcbe0c18f 726
ryuna 5:09afcbe0c18f 727
ryuna 5:09afcbe0c18f 728 }else {
ryuna 5:09afcbe0c18f 729 addDegree = 0;
ryuna 5:09afcbe0c18f 730
ryuna 5:09afcbe0c18f 731 }
ryuna 5:09afcbe0c18f 732
ryuna 5:09afcbe0c18f 733 }
ryuna 5:09afcbe0c18f 734 }else{
ryuna 5:09afcbe0c18f 735 if(IrData[1] > 70){
ryuna 5:09afcbe0c18f 736 if(Degree/180){
ryuna 5:09afcbe0c18f 737 addDegree = 30;
ryuna 5:09afcbe0c18f 738
ryuna 5:09afcbe0c18f 739 }else{
ryuna 5:09afcbe0c18f 740
ryuna 5:09afcbe0c18f 741 addDegree = -30;
ryuna 5:09afcbe0c18f 742 }
ryuna 5:09afcbe0c18f 743
ryuna 5:09afcbe0c18f 744 }else if(IrData[1] >60){
ryuna 5:09afcbe0c18f 745 if(Degree/180){
ryuna 5:09afcbe0c18f 746 addDegree = 20;
ryuna 5:09afcbe0c18f 747
ryuna 5:09afcbe0c18f 748 }else{
ryuna 5:09afcbe0c18f 749 addDegree = -20;
ryuna 5:09afcbe0c18f 750 }
ryuna 5:09afcbe0c18f 751
ryuna 5:09afcbe0c18f 752
ryuna 5:09afcbe0c18f 753 }else {
ryuna 5:09afcbe0c18f 754 addDegree = 0;
ryuna 5:09afcbe0c18f 755
ryuna 5:09afcbe0c18f 756 }
ryuna 5:09afcbe0c18f 757 }
ryuna 5:09afcbe0c18f 758 if(PingData[2]<40){
ryuna 5:09afcbe0c18f 759 if((Degree>150)&&(Degree<300))
ryuna 5:09afcbe0c18f 760 if(Degree/180){
ryuna 5:09afcbe0c18f 761 addDegree = 60;
ryuna 5:09afcbe0c18f 762
ryuna 5:09afcbe0c18f 763 }else{
ryuna 5:09afcbe0c18f 764
ryuna 5:09afcbe0c18f 765 addDegree = -60;
ryuna 5:09afcbe0c18f 766 }
ryuna 5:09afcbe0c18f 767 }
ryuna 5:09afcbe0c18f 768
ryuna 5:09afcbe0c18f 769 Degree = nDegree + addDegree;
ryuna 0:65b9e62cc2b6 770
ryuna 2:e84bb87eea71 771 if((Degree <0)||(Degree>=360)){
ryuna 2:e84bb87eea71 772 Degree = 0;
ryuna 2:e84bb87eea71 773 }
ryuna 2:e84bb87eea71 774 fool(&Degree,&Power);
ryuna 5:09afcbe0c18f 775 move(Power,Power,CompassPID,Degree);
ryuna 2:e84bb87eea71 776
ryuna 2:e84bb87eea71 777 //wait_ms(500);
ryuna 2:e84bb87eea71 778 Led[0] =0;
ryuna 0:65b9e62cc2b6 779 wait_ms(10);
ryuna 0:65b9e62cc2b6 780
ryuna 0:65b9e62cc2b6 781 }
ryuna 5:09afcbe0c18f 782 int i;
ryuna 5:09afcbe0c18f 783 while(1){
ryuna 0:65b9e62cc2b6 784 //デモプログラム
ryuna 3:4a39486ff238 785 //Receive();
ryuna 5:09afcbe0c18f 786 for(i = 0; i<360; i++){
ryuna 5:09afcbe0c18f 787 Power = 20;
ryuna 5:09afcbe0c18f 788 fool(&i,&Power);
ryuna 5:09afcbe0c18f 789
ryuna 5:09afcbe0c18f 790 move(Power,Power,0,i);
ryuna 5:09afcbe0c18f 791 wait_ms(200);
ryuna 5:09afcbe0c18f 792 }
ryuna 5:09afcbe0c18f 793 //pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
ryuna 4:2857f273a7f4 794 //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
ryuna 4:2857f273a7f4 795
ryuna 3:4a39486ff238 796 //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 797 //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
ryuna 2:e84bb87eea71 798 wait(0.1);
ryuna 0:65b9e62cc2b6 799 }
ryuna 0:65b9e62cc2b6 800
ryuna 1:f91d53098d57 801
ryuna 0:65b9e62cc2b6 802
ryuna 0:65b9e62cc2b6 803
ryuna 0:65b9e62cc2b6 804 }