main.cpp

00001 /***********************************
00002  *RoboCupJunior Soccer B Open 2015 
00003  *Koshinnestu progrum
00004  *
00005  *　
00006  *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
00007  *
00008  *　MotorDriverにmaxonに命令
00009  *　
00010  *　servoにステアリング指示
00011  *　
00012  *　LCDでデバック
00013  *
00014  *　スイッチ4つとスタートスイッチで処理を実行
00015  *  
00016 
00017  ************************* 
00018  *  Pin Map 
00019  *  
00020  *  p5~p8   >> BusIn >> LineSensor 
00021  *
00022  *  p9,p10  >> I2C         >> LPC1114FN28/102 read & Compass
00023  *
00024  *  p13,p14 >> Serial      >> Motor
00025  * 
00026  *  p21     >> PwmOut      >> Servo 
00027  *
00028  *  p22~p26 >> DigitalIn   >> DebugSw and StartSw 
00029  *
00030  *  p27,p28 >> I2C         >> DebugLCD
00031  *
00032  *  p29     >> DigitalOut  >> Kicker 
00033  *    
00034  *  *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30
00035  *   
00036  *  
00037  ******************************/
00038 
00039 #include "mbed.h"
00040 #include <math.h>
00041 #include <sstream>
00042 
00043 
00044 #include "Servo.h"
00045 #include "PID.h"
00046 #include "AQM0802A.h"
00047 #include "main.h"
00048 
00049 
00050 void ServoWait(){
00051     WaitFlag = 0;
00052 }
00053 
00054 
00055 void move(int vr,int vl, double vs ,int Degree){
00056     double pwm[4] = {0};
00057     uint8_t i = 0;
00058     static int last_deg;
00059     double vias=30.0;
00060     double dif;
00061     if(abs(last_deg)<23){
00062     
00063         dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
00064         if(abs(dif)<vias){
00065         
00066             pwm[0] = vr;
00067             pwm[1] = 0;
00068             pwm[2] = 0;
00069             pwm[3] = vl;
00070         }
00071         else{
00072             
00073             if(dif<0){
00074                 //vs = vs - (20-vias);
00075                 vs = -(20+(abs(dif))*0.1);
00076                 if( abs(dif) >= 100) vs = -(20+(100)*0.1);
00077             }
00078             else{
00079                 //vs = vs + (20-vias);
00080                 vs = (20+(abs(dif))*0.1);
00081                 if( abs(dif) >= 100) vs = (20+(100)*0.1);
00082             }
00083             
00084             pwm[0] = vs;
00085             pwm[1] = 0;
00086             pwm[2] = 0;
00087             pwm[3] = -vs;
00088         }    
00089     }else{        
00090         pwm[0] = vr;
00091         pwm[1] = 0;
00092         pwm[2] = 0;
00093         pwm[3] = vl;
00094     }
00095     
00096     
00097     if((!WaitFlag)&&(!(last_deg == Degree ))){
00098         SetDegree = Degree * 11/9;
00099         if(SetDegree > 110){
00100             SetDegree = 110;
00101         }else if(SetDegree < -110){
00102             SetDegree = -110;
00103         }
00104         S555.position(SetDegree);
00105         WaitFlag = 1;
00106         STimer.attach(&ServoWait,WAIT_TIME);
00107         
00108         last_deg = Degree;
00109     }   
00110     
00111     
00112     for(i = 0; i < 4; i++){
00113             if(pwm[i] > 100){
00114                 pwm[i] = 100;
00115             }else if(pwm[i] < -100){
00116                 pwm[i] = -100;
00117             }
00118         speed[i] = pwm[i];
00119     }
00120     
00121 
00122     wait_ms(10);
00123 }
00124 
00125 uint8_t PingChange(uint8_t LineData){
00126     static uint8_t Last_Line;
00127     static uint8_t Last_Ping;
00128     uint8_t LinePing = 0;
00129     
00130     if(!LineData){
00131         return 0;
00132     }    
00133 
00134     if(PingData[0] <50) LinePing = LinePing + 1;
00135     if(PingData[1] <50) LinePing = LinePing + 2;
00136     if(PingData[2] <50) LinePing = LinePing + 4;
00137     if(PingData[3] <50) LinePing = LinePing + 8;
00138     
00139     
00140     if(LinePing&0x01){
00141         if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
00142             Last_Ping = LinePing;
00143             Last_Line = LineData;
00144             return 1;
00145         }
00146     }
00147     if(LinePing&0x02){
00148         if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
00149             Last_Ping = LinePing;
00150             Last_Line = LineData;
00151             return 2;
00152         }
00153     }
00154     if(LinePing&0x04){
00155         if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
00156             Last_Ping = LinePing;
00157             Last_Line = LineData;
00158             return 4;
00159         }
00160     }
00161     if(LinePing&0x08){
00162         if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
00163             Last_Ping = LinePing;
00164             Last_Line = LineData;
00165             return 8;
00166         }
00167     }
00168     Last_Ping = 0;
00169     Last_Line = 0;
00170     return 0;
00171     
00172 }
00173 
00174 void fool (int *Degree, int *Power){
00175     static int Last_Degree = 0;
00176     static int Last_Vector = 1;
00177     int degree = *Degree;
00178     int Temp;
00179     
00180     
00181     if((*Degree <0)||(*Degree >=360)){
00182         *Degree = 0;
00183         Last_Degree = 0;
00184         Last_Vector = 1;
00185         return ;
00186     }
00187     
00188     Temp = Last_Degree % 180;
00189     if((Temp>70) &&(Temp<110)){
00190         Temp = *Degree % 180;
00191         if((Temp>70) &&(Temp<110)){
00192             Temp = abs(*Degree - Last_Degree);
00193             if(Temp>160){
00194                 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
00195                 if(*Degree/180){
00196                     *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
00197                     *Power = *Power * Last_Vector;
00198                 }else{
00199                     *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
00200                     *Power = *Power * Last_Vector;
00201                 }
00202                 Last_Degree = degree;
00203                 if((*Degree <= -120)||(*Degree >=120)){
00204                     *Degree = 0;
00205                 }
00206                 return;
00207             }else if((Last_Vector+2) == 1){
00208                 /*逆転のまま角度拡張*/
00209                 if(*Degree/180){
00210                     *Degree = -360 + *Degree ;
00211                     
00212                 }
00213                 *Power = *Power * Last_Vector;
00214                 Last_Degree = degree;
00215                 if((*Degree <= -120)||(*Degree >=120)){
00216                     *Degree = 0;
00217                     
00218                 }
00219                 return;
00220 
00221             }else if((Last_Vector+2) == 3){
00222             /*正転のまま*/
00223                 if(*Degree/180){
00224                     *Degree = -360 + *Degree ;   
00225                 }
00226                 *Power = *Power * Last_Vector;
00227                 Last_Degree = degree;
00228                 if((*Degree <= -120)||(*Degree >=120)){
00229                     *Degree = 0;
00230                 }
00231                 return;
00232             }
00233         }
00234     }
00235     /*通常動作*/
00236     if(*Degree == 0){    
00237         Last_Vector = DegreeToVector[0];
00238         *Degree = Angle[*Degree%180];
00239         *Power = *Power * Last_Vector;
00240         Last_Degree = degree;
00241         return ;
00242     }
00243     if(*Degree == 270){
00244         Last_Vector = DegreeToVector[(*Degree)/90];
00245         *Degree = -90;
00246         *Power = *Power * Last_Vector;
00247         Last_Degree = degree;
00248         return;
00249     }    
00250     Last_Vector = DegreeToVector[(*Degree-1)/90];
00251     *Degree = Angle[*Degree%180];
00252     *Power = *Power * Last_Vector;
00253     Last_Degree = degree;
00254     if((*Degree <= -120)||(*Degree >=120)){
00255         *Degree = 0;
00256     }
00257 
00258 }
00259 int IrDegree(){
00260  /*irの1位の値，2位の場所からirの360へ持っていく*/
00261  
00262     uint8_t IrF ,IrS;
00263     unsigned int irdegree = 0;
00264     if(IrNum >=12){
00265         return 0;   
00266     }
00267     IrF = IrData[0]/12;
00268     IrS = IrData[0]%12;
00269     
00270     if(IrF == 0 ){
00271         if(IrS == 11){
00272             irdegree = 15;
00273                 
00274         }else if(IrS == 1){
00275             irdegree = 345;    
00276         }
00277         return irdegree;
00278     }
00279     irdegree = 360 - IrF*30;    
00280     if(IrS&&(abs(IrF-IrS) == 1)){
00281         irdegree = irdegree - (IrF - IrS)*15;
00282             
00283     }
00284     if(irdegree>=360){
00285         return 0;
00286     }
00287         
00288     return irdegree;
00289 }
00290 
00291 
00292 void IrFrontAction()//ball 12 or 0 o-clock
00293 {
00294     if(IrData[1]>70){
00295         move(30,30,CompassPID,0);
00296         return;
00297     }
00298     if(IrData[1]>60){
00299         move(20,20,CompassPID,0);
00300         return;
00301     }
00302     /*IrData[1]>500*/
00303     move(10,10,0,0);    
00304     if(PingData[0]<50){
00305         if((PingData[1]<60)&&(PingData[1]>40)){
00306         /*右側に居る*/
00307             move(40,25,0,10);
00308             Kick = 1;
00309             wait_ms(200);
00310             move(30,30,0,0);
00311             Kick = 0;
00312             return;
00313         }
00314         if((PingData[1]<60)&&(PingData[1]>40)){
00315         /*左側に居る*/
00316             move(25,40,0,-10);
00317             Kick = 1;
00318             wait_ms(200);
00319             move(30,30,0,0);
00320             Kick = 0;
00321             return;
00322         }
00323         /*それ以外*/
00324         
00325         //Receive();
00326         if(!(IrNum == 0)) return;
00327         move(40,40,CompassPID,0);
00328         Kick = 1;
00329         wait_ms(100);
00330         Kick = 0;
00331         return ;
00332     }
00333     move(20,20,CompassPID,0);
00334     
00335     
00336     
00337 }
00338 
00339 void IrBackAction()//ball found six o-clock
00340 {
00341     /***
00342      * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
00343      *
00344      **/
00345     if(PingData[1]>PingData[3]){
00346         /*右が大きい*/
00347         if(IrData[1]>70){
00348             move(-20,-20,CompassPID,45);
00349             return;
00350         }
00351         if(IrData[1]>60){
00352             move(-20,-20,CompassPID,60);
00353             return;
00354         }        
00355         move(-20,-20,CompassPID,80);
00356         return;
00357     }
00358     /*左が大きい*/
00359     
00360     if(IrData[1]>70){
00361         move(-20,-20,CompassPID,-45);
00362         return;
00363     }
00364     if(IrData[1]>60){
00365         move(-20,-20,CompassPID,-60);
00366         return;
00367     }    
00368     move(-20,-20,CompassPID,-80);
00369     return;
00370 
00371 }
00372 /*
00373 void GoHome()//Ball is not found.
00374 {
00375     int pingRl =0;
00376     int PowLev;
00377     if(PingData[2] >=50){//後ろからの距離60cm
00378         move(-25,-25,CompassPID,0);
00379         return ;
00380     }
00381     if(PingData[2] <= 30){
00382         move(17,17,0,0);
00383         return ;
00384     }
00385     pingRl = PingData[1]+PingData[3];
00386     if(pingRl <100){
00387         move(0,0,CompassPID,0);//stop
00388         return;
00389     }
00390     if(PingData[1] < 52){
00391         PowLev = (52 - PingData[1])*1.35;
00392         move(PowLev,PowLev,0,45);
00393         return ;
00394     }else if (PingData[3] < 52){
00395         PowLev = (52 - PingData[3])*1.35;
00396         move(PowLev,PowLev,0,-45);
00397         return ;
00398     }
00399 
00400     move(0,0,0,0);//stop
00401 
00402 }*/
00403 void GoHome()//Ball is not found.
00404 {
00405     int pingRl =0;
00406     int PowLev;
00407     if(PingData[2] >=70){//後ろからの距離60cm
00408         move(-25,-25,0,0);
00409         return ;
00410     }
00411     
00412     
00413     pingRl = PingData[1]+PingData[3];
00414     if(pingRl <120){
00415         if(PingData[2]<30){
00416             if(PingData[1]<30){
00417                 move(15,15,0,30);
00418             }else if(PingData[3]<30){
00419                 move(15,15,0,-30);
00420             }
00421         }
00422         move(0,0,0,0);//stop
00423         return;
00424     }
00425     if(PingData[1] < 65){
00426         PowLev = (52 - PingData[1])*1.35;
00427         move(-PowLev,-PowLev,0,-20);
00428         return ;
00429     }else if (PingData[3] < 65){
00430         PowLev = (52 - PingData[3])*1.35;
00431         move(-PowLev,-PowLev,0,20);
00432         return ;
00433     }
00434     if(PingData[2]<40){
00435         move(0,0,0,0);//stop
00436     }else{
00437         move(-15,-15,0,0);//stop
00438     
00439     }
00440     
00441     
00442     
00443 }
00444 
00445 
00446 
00447 void PidUpdate()
00448 {   
00449 
00450     pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); 
00451     InputPID = ((Compass/10) + 540 - CompassDef) % 360;
00452  
00453     pid.setProcessValue(InputPID);
00454     CompassPID = (pid.compute());
00455     /*
00456     pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); 
00457     InputPID = Compass;
00458 
00459     pid.setProcessValue(InputPID);
00460     CompassPID = (pid.compute());
00461     */
00462 }
00463 
00464 
00465 
00466 
00467 
00468 
00469 
00470 uint8_t SwRead(){
00471 /******
00472  *retrun : sw_state
00473  *StartS = 0x01;
00474  *Debug2 = 0x02;
00475  *Debug1 = 0x04;
00476  *Debug3 = 0x06;
00477  *Kicker = 0x08;
00478  *Calibration = 0x10;
00479 
00480  *
00481  *****/
00482     uint8_t i,temp,temp2;
00483     temp = ~Sw - 224;
00484     if(!(temp == Kicker
00485        ||temp == Debug1
00486        ||temp == Debug2
00487        ||temp == Debug3
00488        ||temp == Debug12
00489        ||temp == Debug23
00490        ||temp == StartS)) return 0;/*スイッチが押されていない*/
00491     if(!(temp == 0x00)){
00492         for(i = 0; i < 50; i++);
00493         temp2 = ~Sw - 224;
00494         if(temp == temp2){
00495             return temp;
00496         }
00497     }
00498     return 0;
00499 }
00500 
00501 //通信（モータ用）
00502 void tx_motor(){
00503     array(speed[0],speed[1],speed[3],speed[2]);
00504     Motor.printf("%s",StringFIN.c_str());
00505 }
00506 
00507 void SetUp(){
00508     /*初期化*/
00509 
00510     Motor.baud(115200);                             //ボーレート設定
00511     Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
00512     Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
00513     
00514     Mbed.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
00515     
00516     S555.calibrate(0.0006, 120.0);
00517     //S555.position(0.0);    //初期位置にセット
00518     move(0,0,0,0);//停止
00519     
00520     
00521     Kick = 0;
00522     
00523     Sw.mode(PullUp);
00524     
00525     pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
00526     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
00527     pid.setBias(PID_BIAS);                          //pid sed def
00528     pid.setMode(AUTO_MODE);                         //pid sed def
00529     pid.setSetPoint(REFERENCE);                     //pid sed def  
00530 
00531     
00532     for(int i=0; i<15; i++){
00533         CompassDef = Compass/10;
00534         wait_ms(10);
00535     }
00536     //pidupdate.attach(&PidUpdate, PID_CYCLE);
00537     
00538 }
00539 void StartLoop(){
00540 
00541      uint8_t State = 0;
00542      uint8_t LineData = 0;
00543      int a;
00544      while(1){
00545         Led[0] = Led[1] = Led[2] = Led[3] = 1;
00546         //Lcd.cls();
00547         
00548         State = SwRead();
00549         if(State == 0) continue;
00550         
00551         if(State == StartS){
00552             /*loop end & start*/ 
00553            return;
00554         }
00555         
00556         if(State == Debug1){
00557             while((State == Debug1)){
00558                 LineData = (~Line+0x00) & 0x0F;
00559                 Lcd.printf("%d\n",LineData);
00560                 
00561                 wait_ms(100); 
00562                 State = SwRead();
00563             }   
00564             Lcd.cls(); 
00565             continue;
00566             
00567         }
00568         if(State == Debug2){
00569              while((State == Debug2)){
00570                 //Receive();
00571                 Lcd.printf("%d\n",IrNum);
00572                 wait_ms(100);
00573                 State = SwRead();
00574              }
00575              Lcd.cls();
00576              continue;
00577         }
00578 
00579         if(State == Debug3){
00580             while((State == Debug3)){
00581                 a = (Compass/10 + 540 - CompassDef) % 360;
00582                 Lcd.printf("%d\n",a);
00583                 wait_ms(100);
00584                 State = SwRead(); 
00585             }   
00586             Lcd.cls(); 
00587             continue;
00588         }
00589         if(State == Debug12){
00590             while((State == Debug12)){
00591                 Lcd.printf("%d %d\n%d %d\n",PingData[0],PingData[1],PingData[2],PingData[3]);
00592                 wait_ms(100);
00593                 State = SwRead(); 
00594             }   
00595             Lcd.cls(); 
00596             continue;
00597         }
00598         if(State == Kicker){
00599             Led[0] = Led[1] = Led[2] = 0;
00600             Kick = 1;
00601             wait_ms(500);
00602             Kick = 0; 
00603             while((State == Kicker)){
00604                 wait_ms(100); 
00605                 State = SwRead();
00606             }   
00607             continue;   
00608         }
00609     }     
00610     
00611 }
00612 int main() {
00613     
00614     //uint8_t IrNumOld = 0;//過去値
00615     /*Line*/
00616     uint8_t LineData = 0;
00617     uint8_t LinePing = 0;
00618 
00619 
00620     /*State */ 
00621     uint8_t LineIr = 0;
00622     uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
00623                            
00624                            0x04,0x04,0x08,0x08,0x08,0x01,0x00};
00625 
00626     /*行動設定*/
00627     int Power = 0;
00628     int Degree = 0;
00629     
00630     /*楽しい変数達*/
00631     int nDegree =0;//基礎角
00632     int addDegree = 0;//追加角
00633     int Temp = 0;
00634     
00635     int  StopCt = 0;
00636     
00637     /*関数ポインタ*/
00638 
00639 
00640     //void (*AnotherAction[3])(uint8_t [],double);
00641     void (*AnotherAction[3])();
00642     AnotherAction[0] = IrFrontAction;
00643     AnotherAction[1] = IrBackAction;
00644     AnotherAction[2] = GoHome;
00645     
00646 
00647     SetUp();/*set up routine*/
00648 
00649     StartLoop(); /*loop strat, switch push end*/
00650     Led[0] = Led[1] = Led[2] = Led[3] = 0;
00651     wait_ms(100);
00652     
00653     while(1){
00654          
00655         //S555.calibrate(0.0006, 120.0);
00656 
00657         //Receive();
00658         //Lcd.printf("%d\n",IrNum);
00659         /*白線を読んでいないか確認する*/
00660         
00661         LineData = (~Line+0x00) & 0x0F; 
00662         
00663         if(LineData){
00664             LineIr = LineData & IrChange[IrNum];
00665             LinePing = PingChange(LineData);
00666             if(LinePing){
00667                 move(0,0,0,0);
00668                 wait_ms(200);
00669             }
00670             if(LineIr){
00671                 move(0,0,0,0);
00672                 StopCt = 0;
00673                 while(LineIr){
00674                     if(StopCt>30){
00675                         GoHome();
00676                         wait_ms(100);
00677                     }
00678                     StopCt++;
00679                     Led[1] = Led[2] = Led[3] = 1;  
00680                     //Receive();
00681                     LineData = (~Line+0x00) & 0x0F;
00682                     LineIr = LineData & IrChange[IrNum];
00683                     wait_ms(10);
00684                 }                 
00685             }else if(LinePing){
00686                 move(0,0,0,0);
00687                 StopCt = 0;
00688                 while(LinePing){
00689                     if(StopCt>10){
00690                         GoHome();
00691                         wait_ms(200);
00692                     }
00693                     StopCt++;
00694                     
00695                     Led[1] = Led[2] = Led[3] = 1;  
00696                     //Receive();
00697                     LineData = (~Line+0x00) & 0x0F;
00698                     LinePing = PingChange(LineData);
00699                     
00700                     wait_ms(10);
00701                 }
00702             }
00703             
00704             
00705             Led[1] = Led[2] = Led[3] = 0;
00706             
00707         }
00708         
00709         Power = 0;
00710         Led[0] = 1;
00711         Degree = 0;
00712         SetC = 0.0;
00713         
00714         
00715         Led[3] = 1;
00716         
00717         Degree = IrDegree();
00718         
00719         if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
00720             (AnotherAction[IrNum/6])();
00721             continue;
00722         }
00723         
00724         /*
00725         if(IrNum == 12){
00726             move(0,0,0,0);
00727             wait_ms(10);
00728             continue;   
00729         }*/
00730         
00731         nDegree = wrapDegree[Degree/15];
00732         Power = 30;
00733         Temp = Degree%180;
00734         
00735         if((Temp>70)&&(Temp<110)){
00736             if(PingData[2]<40){
00737                 if(IrData[1] > 70){
00738                     if(Degree/180){
00739                         addDegree = 90;     
00740                     }else{
00741                         addDegree = -90;
00742                     }   
00743                 }else if(IrData[1] >60){
00744                     if(Degree/180){
00745                         addDegree = 60;     
00746                     }else{
00747                         addDegree = -60;
00748                     }
00749                 }else{  
00750                     addDegree = 0;
00751                 }
00752             }else{
00753                 if(IrData[1] > 70){
00754                     if(Degree/180){
00755                     addDegree = 30;    
00756                     
00757                     }else{
00758                     
00759                     addDegree = -30;
00760                     }
00761                     
00762                 }else if(IrData[1] >60){
00763                     if(Degree/180){
00764                     addDegree = 20;    
00765                     
00766                     }else{
00767                     addDegree = -20;
00768                    }
00769                   
00770                 
00771                 }else {
00772                     addDegree = 0;
00773                     
00774                 } 
00775                     
00776             }
00777         }else{   
00778             if(IrData[1] > 70){
00779                 if(Degree/180){
00780                 addDegree = 30;    
00781                 
00782                 }else{
00783                 
00784                 addDegree = -30;
00785                 }
00786                 
00787             }else if(IrData[1] >60){
00788                 if(Degree/180){
00789                 addDegree = 20;    
00790                 
00791                 }else{
00792                 addDegree = -20;
00793                }
00794               
00795             
00796             }else {
00797                 addDegree = 0;
00798                 
00799             }
00800         }    
00801         if(PingData[2]<40){
00802             if((Degree>150)&&(Degree<300))
00803                 if(Degree/180){
00804                     addDegree = 60;    
00805                 
00806                 }else{
00807                 
00808                     addDegree = -60;
00809             }
00810         }
00811                 
00812         Degree = nDegree + addDegree;
00813         
00814         if((Degree <0)||(Degree>=360)){
00815             Degree = 0;
00816         }
00817         fool(&Degree,&Power);
00818         move(Power,Power,CompassPID,Degree);
00819         
00820         //wait_ms(500);
00821         Led[0] =0;
00822         wait_ms(10);
00823         
00824     }
00825     int i;
00826     while(1){
00827     //デモプログラム
00828         //Receive();
00829         for(i = 0; i<360; i++){
00830             Power = 20;
00831             fool(&i,&Power);
00832             
00833             move(Power,Power,0,i);
00834             wait_ms(200);
00835         }
00836         //pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
00837         //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
00838         
00839         //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]);
00840         //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
00841         wait(0.1);
00842     }
00843     
00844     
00845     
00846     
00847 }