CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

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

Dependencies:   AQM0802A PID Servo mbed

main.cpp@7:01bb0b0a98f1, 2015-03-14 (annotated)

Committer:
ryuna
Date:
Sat Mar 14 00:47:16 2015 +0000
Revision:
7:01bb0b0a98f1
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 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 7:01bb0b0a98f1 97 if((!WaitFlag)&&(!(last_deg == Degree ))){
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 7:01bb0b0a98f1 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 7:01bb0b0a98f1 391 PowLev = (52 - PingData[1])*1.35;
ryuna 7:01bb0b0a98f1 392 move(PowLev,PowLev,0,45);
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 7:01bb0b0a98f1 396 move(PowLev,PowLev,0,-45);
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 7:01bb0b0a98f1 402 }*/
ryuna 7:01bb0b0a98f1 403 void GoHome()//Ball is not found.
ryuna 7:01bb0b0a98f1 404 {
ryuna 7:01bb0b0a98f1 405 int pingRl =0;
ryuna 7:01bb0b0a98f1 406 int PowLev;
ryuna 7:01bb0b0a98f1 407 if(PingData[2] >=70){//後ろからの距離60cm
ryuna 7:01bb0b0a98f1 408 move(-25,-25,0,0);
ryuna 7:01bb0b0a98f1 409 return ;
ryuna 7:01bb0b0a98f1 410 }
ryuna 7:01bb0b0a98f1 411
ryuna 7:01bb0b0a98f1 412
ryuna 7:01bb0b0a98f1 413 pingRl = PingData[1]+PingData[3];
ryuna 7:01bb0b0a98f1 414 if(pingRl <120){
ryuna 7:01bb0b0a98f1 415 if(PingData[2]<30){
ryuna 7:01bb0b0a98f1 416 if(PingData[1]<30){
ryuna 7:01bb0b0a98f1 417 move(15,15,0,30);
ryuna 7:01bb0b0a98f1 418 }else if(PingData[3]<30){
ryuna 7:01bb0b0a98f1 419 move(15,15,0,-30);
ryuna 7:01bb0b0a98f1 420 }
ryuna 7:01bb0b0a98f1 421 }
ryuna 7:01bb0b0a98f1 422 move(0,0,0,0);//stop
ryuna 7:01bb0b0a98f1 423 return;
ryuna 7:01bb0b0a98f1 424 }
ryuna 7:01bb0b0a98f1 425 if(PingData[1] < 65){
ryuna 7:01bb0b0a98f1 426 PowLev = (52 - PingData[1])*1.35;
ryuna 7:01bb0b0a98f1 427 move(-PowLev,-PowLev,0,-20);
ryuna 7:01bb0b0a98f1 428 return ;
ryuna 7:01bb0b0a98f1 429 }else if (PingData[3] < 65){
ryuna 7:01bb0b0a98f1 430 PowLev = (52 - PingData[3])*1.35;
ryuna 7:01bb0b0a98f1 431 move(-PowLev,-PowLev,0,20);
ryuna 7:01bb0b0a98f1 432 return ;
ryuna 7:01bb0b0a98f1 433 }
ryuna 7:01bb0b0a98f1 434 if(PingData[2]<40){
ryuna 7:01bb0b0a98f1 435 move(0,0,0,0);//stop
ryuna 7:01bb0b0a98f1 436 }else{
ryuna 7:01bb0b0a98f1 437 move(-15,-15,0,0);//stop
ryuna 7:01bb0b0a98f1 438
ryuna 7:01bb0b0a98f1 439 }
ryuna 7:01bb0b0a98f1 440
ryuna 7:01bb0b0a98f1 441
ryuna 7:01bb0b0a98f1 442
ryuna 0:65b9e62cc2b6 443 }
ryuna 0:65b9e62cc2b6 444
ryuna 0:65b9e62cc2b6 445
ryuna 7:01bb0b0a98f1 446
ryuna 0:65b9e62cc2b6 447 void PidUpdate()
ryuna 0:65b9e62cc2b6 448 {
ryuna 5:09afcbe0c18f 449
ryuna 5:09afcbe0c18f 450 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 5:09afcbe0c18f 451 InputPID = ((Compass/10) + 540 - CompassDef) % 360;
ryuna 5:09afcbe0c18f 452
ryuna 5:09afcbe0c18f 453 pid.setProcessValue(InputPID);
ryuna 5:09afcbe0c18f 454 CompassPID = (pid.compute());
ryuna 5:09afcbe0c18f 455 /*
ryuna 2:e84bb87eea71 456 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
ryuna 2:e84bb87eea71 457 InputPID = Compass;
ryuna 2:e84bb87eea71 458
ryuna 2:e84bb87eea71 459 pid.setProcessValue(InputPID);
ryuna 4:2857f273a7f4 460 CompassPID = (pid.compute());
ryuna 5:09afcbe0c18f 461 */
ryuna 0:65b9e62cc2b6 462 }
ryuna 0:65b9e62cc2b6 463
ryuna 0:65b9e62cc2b6 464
ryuna 0:65b9e62cc2b6 465
ryuna 0:65b9e62cc2b6 466
ryuna 0:65b9e62cc2b6 467
ryuna 0:65b9e62cc2b6 468
ryuna 0:65b9e62cc2b6 469
ryuna 0:65b9e62cc2b6 470 uint8_t SwRead(){
ryuna 0:65b9e62cc2b6 471 /******
ryuna 0:65b9e62cc2b6 472 *retrun : sw_state
ryuna 0:65b9e62cc2b6 473 *StartS = 0x01;
ryuna 0:65b9e62cc2b6 474 *Debug2 = 0x02;
ryuna 0:65b9e62cc2b6 475 *Debug1 = 0x04;
ryuna 0:65b9e62cc2b6 476 *Debug3 = 0x06;
ryuna 0:65b9e62cc2b6 477 *Kicker = 0x08;
ryuna 0:65b9e62cc2b6 478 *Calibration = 0x10;
ryuna 0:65b9e62cc2b6 479
ryuna 0:65b9e62cc2b6 480 *
ryuna 0:65b9e62cc2b6 481 *****/
ryuna 0:65b9e62cc2b6 482 uint8_t i,temp,temp2;
ryuna 0:65b9e62cc2b6 483 temp = ~Sw - 224;
ryuna 0:65b9e62cc2b6 484 if(!(temp == Kicker
ryuna 0:65b9e62cc2b6 485 ||temp == Debug1
ryuna 0:65b9e62cc2b6 486 ||temp == Debug2
ryuna 0:65b9e62cc2b6 487 ||temp == Debug3
ryuna 5:09afcbe0c18f 488 ||temp == Debug12
ryuna 5:09afcbe0c18f 489 ||temp == Debug23
ryuna 0:65b9e62cc2b6 490 ||temp == StartS)) return 0;/*スイッチが押されていない*/
ryuna 0:65b9e62cc2b6 491 if(!(temp == 0x00)){
ryuna 0:65b9e62cc2b6 492 for(i = 0; i < 50; i++);
ryuna 0:65b9e62cc2b6 493 temp2 = ~Sw - 224;
ryuna 0:65b9e62cc2b6 494 if(temp == temp2){
ryuna 0:65b9e62cc2b6 495 return temp;
ryuna 0:65b9e62cc2b6 496 }
ryuna 0:65b9e62cc2b6 497 }
ryuna 0:65b9e62cc2b6 498 return 0;
ryuna 0:65b9e62cc2b6 499 }
ryuna 0:65b9e62cc2b6 500
ryuna 0:65b9e62cc2b6 501 //通信（モータ用）
ryuna 0:65b9e62cc2b6 502 void tx_motor(){
ryuna 0:65b9e62cc2b6 503 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:65b9e62cc2b6 504 Motor.printf("%s",StringFIN.c_str());
ryuna 0:65b9e62cc2b6 505 }
ryuna 0:65b9e62cc2b6 506
ryuna 0:65b9e62cc2b6 507 void SetUp(){
ryuna 0:65b9e62cc2b6 508 /*初期化*/
ryuna 3:4a39486ff238 509
ryuna 0:65b9e62cc2b6 510 Motor.baud(115200); //ボーレート設定
ryuna 0:65b9e62cc2b6 511 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:65b9e62cc2b6 512 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:65b9e62cc2b6 513
ryuna 4:2857f273a7f4 514 Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
ryuna 4:2857f273a7f4 515
ryuna 5:09afcbe0c18f 516 S555.calibrate(0.0006, 120.0);
ryuna 2:e84bb87eea71 517 //S555.position(0.0); //初期位置にセット
ryuna 0:65b9e62cc2b6 518 move(0,0,0,0);//停止
ryuna 0:65b9e62cc2b6 519
ryuna 5:09afcbe0c18f 520
ryuna 0:65b9e62cc2b6 521 Kick = 0;
ryuna 5:09afcbe0c18f 522
ryuna 0:65b9e62cc2b6 523 Sw.mode(PullUp);
ryuna 0:65b9e62cc2b6 524
ryuna 0:65b9e62cc2b6 525 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
ryuna 0:65b9e62cc2b6 526 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
ryuna 0:65b9e62cc2b6 527 pid.setBias(PID_BIAS); //pid sed def
ryuna 0:65b9e62cc2b6 528 pid.setMode(AUTO_MODE); //pid sed def
ryuna 0:65b9e62cc2b6 529 pid.setSetPoint(REFERENCE); //pid sed def
ryuna 1:f91d53098d57 530
ryuna 0:65b9e62cc2b6 531
ryuna 5:09afcbe0c18f 532 for(int i=0; i<15; i++){
ryuna 5:09afcbe0c18f 533 CompassDef = Compass/10;
ryuna 5:09afcbe0c18f 534 wait_ms(10);
ryuna 5:09afcbe0c18f 535 }
ryuna 5:09afcbe0c18f 536 //pidupdate.attach(&PidUpdate, PID_CYCLE);
ryuna 5:09afcbe0c18f 537
ryuna 0:65b9e62cc2b6 538 }
ryuna 0:65b9e62cc2b6 539 void StartLoop(){
ryuna 0:65b9e62cc2b6 540
ryuna 0:65b9e62cc2b6 541 uint8_t State = 0;
ryuna 0:65b9e62cc2b6 542 uint8_t LineData = 0;
ryuna 5:09afcbe0c18f 543 int a;
ryuna 0:65b9e62cc2b6 544 while(1){
ryuna 0:65b9e62cc2b6 545 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 0:65b9e62cc2b6 546 //Lcd.cls();
ryuna 2:e84bb87eea71 547
ryuna 0:65b9e62cc2b6 548 State = SwRead();
ryuna 0:65b9e62cc2b6 549 if(State == 0) continue;
ryuna 0:65b9e62cc2b6 550
ryuna 0:65b9e62cc2b6 551 if(State == StartS){
ryuna 0:65b9e62cc2b6 552 /*loop end & start*/
ryuna 0:65b9e62cc2b6 553 return;
ryuna 0:65b9e62cc2b6 554 }
ryuna 0:65b9e62cc2b6 555
ryuna 0:65b9e62cc2b6 556 if(State == Debug1){
ryuna 0:65b9e62cc2b6 557 while((State == Debug1)){
ryuna 0:65b9e62cc2b6 558 LineData = (~Line+0x00) & 0x0F;
ryuna 0:65b9e62cc2b6 559 Lcd.printf("%d\n",LineData);
ryuna 0:65b9e62cc2b6 560
ryuna 0:65b9e62cc2b6 561 wait_ms(100);
ryuna 0:65b9e62cc2b6 562 State = SwRead();
ryuna 0:65b9e62cc2b6 563 }
ryuna 0:65b9e62cc2b6 564 Lcd.cls();
ryuna 0:65b9e62cc2b6 565 continue;
ryuna 0:65b9e62cc2b6 566
ryuna 0:65b9e62cc2b6 567 }
ryuna 0:65b9e62cc2b6 568 if(State == Debug2){
ryuna 0:65b9e62cc2b6 569 while((State == Debug2)){
ryuna 4:2857f273a7f4 570 //Receive();
ryuna 2:e84bb87eea71 571 Lcd.printf("%d\n",IrNum);
ryuna 0:65b9e62cc2b6 572 wait_ms(100);
ryuna 0:65b9e62cc2b6 573 State = SwRead();
ryuna 0:65b9e62cc2b6 574 }
ryuna 0:65b9e62cc2b6 575 Lcd.cls();
ryuna 0:65b9e62cc2b6 576 continue;
ryuna 0:65b9e62cc2b6 577 }
ryuna 0:65b9e62cc2b6 578
ryuna 0:65b9e62cc2b6 579 if(State == Debug3){
ryuna 0:65b9e62cc2b6 580 while((State == Debug3)){
ryuna 5:09afcbe0c18f 581 a = (Compass/10 + 540 - CompassDef) % 360;
ryuna 5:09afcbe0c18f 582 Lcd.printf("%d\n",a);
ryuna 0:65b9e62cc2b6 583 wait_ms(100);
ryuna 0:65b9e62cc2b6 584 State = SwRead();
ryuna 0:65b9e62cc2b6 585 }
ryuna 0:65b9e62cc2b6 586 Lcd.cls();
ryuna 0:65b9e62cc2b6 587 continue;
ryuna 0:65b9e62cc2b6 588 }
ryuna 5:09afcbe0c18f 589 if(State == Debug12){
ryuna 5:09afcbe0c18f 590 while((State == Debug12)){
ryuna 5:09afcbe0c18f 591 Lcd.printf("%d %d\n%d %d\n",PingData[0],PingData[1],PingData[2],PingData[3]);
ryuna 5:09afcbe0c18f 592 wait_ms(100);
ryuna 5:09afcbe0c18f 593 State = SwRead();
ryuna 5:09afcbe0c18f 594 }
ryuna 5:09afcbe0c18f 595 Lcd.cls();
ryuna 5:09afcbe0c18f 596 continue;
ryuna 5:09afcbe0c18f 597 }
ryuna 0:65b9e62cc2b6 598 if(State == Kicker){
ryuna 0:65b9e62cc2b6 599 Led[0] = Led[1] = Led[2] = 0;
ryuna 4:2857f273a7f4 600 Kick = 1;
ryuna 4:2857f273a7f4 601 wait_ms(500);
ryuna 4:2857f273a7f4 602 Kick = 0;
ryuna 0:65b9e62cc2b6 603 while((State == Kicker)){
ryuna 0:65b9e62cc2b6 604 wait_ms(100);
ryuna 0:65b9e62cc2b6 605 State = SwRead();
ryuna 0:65b9e62cc2b6 606 }
ryuna 0:65b9e62cc2b6 607 continue;
ryuna 0:65b9e62cc2b6 608 }
ryuna 0:65b9e62cc2b6 609 }
ryuna 0:65b9e62cc2b6 610
ryuna 0:65b9e62cc2b6 611 }
ryuna 0:65b9e62cc2b6 612 int main() {
ryuna 0:65b9e62cc2b6 613
ryuna 0:65b9e62cc2b6 614 //uint8_t IrNumOld = 0;//過去値
ryuna 0:65b9e62cc2b6 615 /*Line*/
ryuna 0:65b9e62cc2b6 616 uint8_t LineData = 0;
ryuna 2:e84bb87eea71 617 uint8_t LinePing = 0;
ryuna 0:65b9e62cc2b6 618
ryuna 2:e84bb87eea71 619
ryuna 0:65b9e62cc2b6 620 /*State */
ryuna 0:65b9e62cc2b6 621 uint8_t LineIr = 0;
ryuna 5:09afcbe0c18f 622 uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
ryuna 3:4a39486ff238 623
ryuna 5:09afcbe0c18f 624 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
ryuna 2:e84bb87eea71 625
ryuna 0:65b9e62cc2b6 626 /*行動設定*/
ryuna 0:65b9e62cc2b6 627 int Power = 0;
ryuna 2:e84bb87eea71 628 int Degree = 0;
ryuna 0:65b9e62cc2b6 629
ryuna 0:65b9e62cc2b6 630 /*楽しい変数達*/
ryuna 2:e84bb87eea71 631 int nDegree =0;//基礎角
ryuna 2:e84bb87eea71 632 int addDegree = 0;//追加角
ryuna 5:09afcbe0c18f 633 int Temp = 0;
ryuna 0:65b9e62cc2b6 634
ryuna 5:09afcbe0c18f 635 int StopCt = 0;
ryuna 0:65b9e62cc2b6 636
ryuna 0:65b9e62cc2b6 637 /*関数ポインタ*/
ryuna 2:e84bb87eea71 638
ryuna 2:e84bb87eea71 639
ryuna 2:e84bb87eea71 640 //void (*AnotherAction[3])(uint8_t [],double);
ryuna 2:e84bb87eea71 641 void (*AnotherAction[3])();
ryuna 0:65b9e62cc2b6 642 AnotherAction[0] = IrFrontAction;
ryuna 0:65b9e62cc2b6 643 AnotherAction[1] = IrBackAction;
ryuna 0:65b9e62cc2b6 644 AnotherAction[2] = GoHome;
ryuna 1:f91d53098d57 645
ryuna 2:e84bb87eea71 646
ryuna 0:65b9e62cc2b6 647 SetUp();/*set up routine*/
ryuna 4:2857f273a7f4 648
ryuna 0:65b9e62cc2b6 649 StartLoop(); /*loop strat, switch push end*/
ryuna 2:e84bb87eea71 650 Led[0] = Led[1] = Led[2] = Led[3] = 0;
ryuna 2:e84bb87eea71 651 wait_ms(100);
ryuna 0:65b9e62cc2b6 652
ryuna 4:2857f273a7f4 653 while(1){
ryuna 0:65b9e62cc2b6 654
ryuna 5:09afcbe0c18f 655 //S555.calibrate(0.0006, 120.0);
ryuna 2:e84bb87eea71 656
ryuna 4:2857f273a7f4 657 //Receive();
ryuna 2:e84bb87eea71 658 //Lcd.printf("%d\n",IrNum);
ryuna 0:65b9e62cc2b6 659 /*白線を読んでいないか確認する*/
ryuna 5:09afcbe0c18f 660
ryuna 0:65b9e62cc2b6 661 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 662
ryuna 0:65b9e62cc2b6 663 if(LineData){
ryuna 2:e84bb87eea71 664 LineIr = LineData & IrChange[IrNum];
ryuna 2:e84bb87eea71 665 LinePing = PingChange(LineData);
ryuna 5:09afcbe0c18f 666 if(LinePing){
ryuna 5:09afcbe0c18f 667 move(0,0,0,0);
ryuna 5:09afcbe0c18f 668 wait_ms(200);
ryuna 5:09afcbe0c18f 669 }
ryuna 2:e84bb87eea71 670 if(LineIr){
ryuna 2:e84bb87eea71 671 move(0,0,0,0);
ryuna 5:09afcbe0c18f 672 StopCt = 0;
ryuna 2:e84bb87eea71 673 while(LineIr){
ryuna 5:09afcbe0c18f 674 if(StopCt>30){
ryuna 5:09afcbe0c18f 675 GoHome();
ryuna 5:09afcbe0c18f 676 wait_ms(100);
ryuna 5:09afcbe0c18f 677 }
ryuna 5:09afcbe0c18f 678 StopCt++;
ryuna 2:e84bb87eea71 679 Led[1] = Led[2] = Led[3] = 1;
ryuna 4:2857f273a7f4 680 //Receive();
ryuna 2:e84bb87eea71 681 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 682 LineIr = LineData & IrChange[IrNum];
ryuna 2:e84bb87eea71 683 wait_ms(10);
ryuna 2:e84bb87eea71 684 }
ryuna 2:e84bb87eea71 685 }else if(LinePing){
ryuna 3:4a39486ff238 686 move(0,0,0,0);
ryuna 5:09afcbe0c18f 687 StopCt = 0;
ryuna 2:e84bb87eea71 688 while(LinePing){
ryuna 7:01bb0b0a98f1 689 if(StopCt>10){
ryuna 5:09afcbe0c18f 690 GoHome();
ryuna 5:09afcbe0c18f 691 wait_ms(200);
ryuna 5:09afcbe0c18f 692 }
ryuna 5:09afcbe0c18f 693 StopCt++;
ryuna 7:01bb0b0a98f1 694
ryuna 2:e84bb87eea71 695 Led[1] = Led[2] = Led[3] = 1;
ryuna 4:2857f273a7f4 696 //Receive();
ryuna 2:e84bb87eea71 697 LineData = (~Line+0x00) & 0x0F;
ryuna 2:e84bb87eea71 698 LinePing = PingChange(LineData);
ryuna 3:4a39486ff238 699
ryuna 2:e84bb87eea71 700 wait_ms(10);
ryuna 2:e84bb87eea71 701 }
ryuna 0:65b9e62cc2b6 702 }
ryuna 2:e84bb87eea71 703
ryuna 2:e84bb87eea71 704
ryuna 0:65b9e62cc2b6 705 Led[1] = Led[2] = Led[3] = 0;
ryuna 0:65b9e62cc2b6 706
ryuna 0:65b9e62cc2b6 707 }
ryuna 0:65b9e62cc2b6 708
ryuna 2:e84bb87eea71 709 Power = 0;
ryuna 2:e84bb87eea71 710 Led[0] = 1;
ryuna 2:e84bb87eea71 711 Degree = 0;
ryuna 2:e84bb87eea71 712 SetC = 0.0;
ryuna 2:e84bb87eea71 713
ryuna 0:65b9e62cc2b6 714
ryuna 0:65b9e62cc2b6 715 Led[3] = 1;
ryuna 5:09afcbe0c18f 716
ryuna 2:e84bb87eea71 717 Degree = IrDegree();
ryuna 0:65b9e62cc2b6 718
ryuna 2:e84bb87eea71 719 if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
ryuna 2:e84bb87eea71 720 (AnotherAction[IrNum/6])();
ryuna 0:65b9e62cc2b6 721 continue;
ryuna 0:65b9e62cc2b6 722 }
ryuna 2:e84bb87eea71 723
ryuna 2:e84bb87eea71 724 /*
ryuna 2:e84bb87eea71 725 if(IrNum == 12){
ryuna 2:e84bb87eea71 726 move(0,0,0,0);
ryuna 2:e84bb87eea71 727 wait_ms(10);
ryuna 2:e84bb87eea71 728 continue;
ryuna 2:e84bb87eea71 729 }*/
ryuna 2:e84bb87eea71 730
ryuna 2:e84bb87eea71 731 nDegree = wrapDegree[Degree/15];
ryuna 5:09afcbe0c18f 732 Power = 30;
ryuna 5:09afcbe0c18f 733 Temp = Degree%180;
ryuna 0:65b9e62cc2b6 734
ryuna 5:09afcbe0c18f 735 if((Temp>70)&&(Temp<110)){
ryuna 5:09afcbe0c18f 736 if(PingData[2]<40){
ryuna 5:09afcbe0c18f 737 if(IrData[1] > 70){
ryuna 5:09afcbe0c18f 738 if(Degree/180){
ryuna 5:09afcbe0c18f 739 addDegree = 90;
ryuna 5:09afcbe0c18f 740 }else{
ryuna 5:09afcbe0c18f 741 addDegree = -90;
ryuna 5:09afcbe0c18f 742 }
ryuna 5:09afcbe0c18f 743 }else if(IrData[1] >60){
ryuna 5:09afcbe0c18f 744 if(Degree/180){
ryuna 5:09afcbe0c18f 745 addDegree = 60;
ryuna 5:09afcbe0c18f 746 }else{
ryuna 5:09afcbe0c18f 747 addDegree = -60;
ryuna 5:09afcbe0c18f 748 }
ryuna 5:09afcbe0c18f 749 }else{
ryuna 5:09afcbe0c18f 750 addDegree = 0;
ryuna 5:09afcbe0c18f 751 }
ryuna 5:09afcbe0c18f 752 }else{
ryuna 5:09afcbe0c18f 753 if(IrData[1] > 70){
ryuna 5:09afcbe0c18f 754 if(Degree/180){
ryuna 5:09afcbe0c18f 755 addDegree = 30;
ryuna 5:09afcbe0c18f 756
ryuna 5:09afcbe0c18f 757 }else{
ryuna 5:09afcbe0c18f 758
ryuna 5:09afcbe0c18f 759 addDegree = -30;
ryuna 5:09afcbe0c18f 760 }
ryuna 5:09afcbe0c18f 761
ryuna 5:09afcbe0c18f 762 }else if(IrData[1] >60){
ryuna 5:09afcbe0c18f 763 if(Degree/180){
ryuna 5:09afcbe0c18f 764 addDegree = 20;
ryuna 5:09afcbe0c18f 765
ryuna 5:09afcbe0c18f 766 }else{
ryuna 5:09afcbe0c18f 767 addDegree = -20;
ryuna 5:09afcbe0c18f 768 }
ryuna 5:09afcbe0c18f 769
ryuna 5:09afcbe0c18f 770
ryuna 5:09afcbe0c18f 771 }else {
ryuna 5:09afcbe0c18f 772 addDegree = 0;
ryuna 5:09afcbe0c18f 773
ryuna 5:09afcbe0c18f 774 }
ryuna 5:09afcbe0c18f 775
ryuna 5:09afcbe0c18f 776 }
ryuna 5:09afcbe0c18f 777 }else{
ryuna 5:09afcbe0c18f 778 if(IrData[1] > 70){
ryuna 5:09afcbe0c18f 779 if(Degree/180){
ryuna 5:09afcbe0c18f 780 addDegree = 30;
ryuna 5:09afcbe0c18f 781
ryuna 5:09afcbe0c18f 782 }else{
ryuna 5:09afcbe0c18f 783
ryuna 5:09afcbe0c18f 784 addDegree = -30;
ryuna 5:09afcbe0c18f 785 }
ryuna 5:09afcbe0c18f 786
ryuna 5:09afcbe0c18f 787 }else if(IrData[1] >60){
ryuna 5:09afcbe0c18f 788 if(Degree/180){
ryuna 5:09afcbe0c18f 789 addDegree = 20;
ryuna 5:09afcbe0c18f 790
ryuna 5:09afcbe0c18f 791 }else{
ryuna 5:09afcbe0c18f 792 addDegree = -20;
ryuna 5:09afcbe0c18f 793 }
ryuna 5:09afcbe0c18f 794
ryuna 5:09afcbe0c18f 795
ryuna 5:09afcbe0c18f 796 }else {
ryuna 5:09afcbe0c18f 797 addDegree = 0;
ryuna 5:09afcbe0c18f 798
ryuna 5:09afcbe0c18f 799 }
ryuna 5:09afcbe0c18f 800 }
ryuna 5:09afcbe0c18f 801 if(PingData[2]<40){
ryuna 5:09afcbe0c18f 802 if((Degree>150)&&(Degree<300))
ryuna 5:09afcbe0c18f 803 if(Degree/180){
ryuna 5:09afcbe0c18f 804 addDegree = 60;
ryuna 5:09afcbe0c18f 805
ryuna 5:09afcbe0c18f 806 }else{
ryuna 5:09afcbe0c18f 807
ryuna 5:09afcbe0c18f 808 addDegree = -60;
ryuna 5:09afcbe0c18f 809 }
ryuna 5:09afcbe0c18f 810 }
ryuna 5:09afcbe0c18f 811
ryuna 5:09afcbe0c18f 812 Degree = nDegree + addDegree;
ryuna 0:65b9e62cc2b6 813
ryuna 2:e84bb87eea71 814 if((Degree <0)||(Degree>=360)){
ryuna 2:e84bb87eea71 815 Degree = 0;
ryuna 2:e84bb87eea71 816 }
ryuna 2:e84bb87eea71 817 fool(&Degree,&Power);
ryuna 5:09afcbe0c18f 818 move(Power,Power,CompassPID,Degree);
ryuna 2:e84bb87eea71 819
ryuna 2:e84bb87eea71 820 //wait_ms(500);
ryuna 2:e84bb87eea71 821 Led[0] =0;
ryuna 0:65b9e62cc2b6 822 wait_ms(10);
ryuna 0:65b9e62cc2b6 823
ryuna 0:65b9e62cc2b6 824 }
ryuna 5:09afcbe0c18f 825 int i;
ryuna 5:09afcbe0c18f 826 while(1){
ryuna 0:65b9e62cc2b6 827 //デモプログラム
ryuna 3:4a39486ff238 828 //Receive();
ryuna 5:09afcbe0c18f 829 for(i = 0; i<360; i++){
ryuna 5:09afcbe0c18f 830 Power = 20;
ryuna 5:09afcbe0c18f 831 fool(&i,&Power);
ryuna 5:09afcbe0c18f 832
ryuna 5:09afcbe0c18f 833 move(Power,Power,0,i);
ryuna 5:09afcbe0c18f 834 wait_ms(200);
ryuna 5:09afcbe0c18f 835 }
ryuna 5:09afcbe0c18f 836 //pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
ryuna 4:2857f273a7f4 837 //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
ryuna 4:2857f273a7f4 838
ryuna 3:4a39486ff238 839 //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 840 //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
ryuna 2:e84bb87eea71 841 wait(0.1);
ryuna 0:65b9e62cc2b6 842 }
ryuna 0:65b9e62cc2b6 843
ryuna 1:f91d53098d57 844
ryuna 0:65b9e62cc2b6 845
ryuna 0:65b9e62cc2b6 846
ryuna 0:65b9e62cc2b6 847 }