main.cpp

00001 #include <math.h>
00002 #include <sstream>
00003 #include "mbed.h"
00004 #include "HMC6352.h"
00005 #include "PID.h"
00006 #include "main.h"
00007 
00008 
00009 void whiteliner()
00010 {
00011     static int tmp[5] = {0};
00012     static int sum = 0;
00013     
00014     sum -= tmp[4];
00015     sum += whiteout_flag;
00016     tmp[4] = tmp[3];
00017     tmp[3] = tmp[2];
00018     tmp[2] = tmp[1];
00019     tmp[1] = tmp[0];
00020     tmp[0] = whiteout_flag;
00021     
00022     //return sum/5;
00023 }
00024 
00025 int hansya_x(int x)
00026 {
00027     static int tmp[5] = {0};
00028     static int sum = 0;
00029     
00030     sum -= tmp[4];
00031     sum += x;
00032     tmp[4] = tmp[3];
00033     tmp[3] = tmp[2];
00034     tmp[2] = tmp[1];
00035     tmp[1] = tmp[0];
00036     tmp[0] = x;
00037     
00038     return sum/5;
00039 }
00040 
00041 
00042 int hansya_y(int y)
00043 {
00044     static int tmp[5] = {0};
00045     static int sum = 0;
00046     
00047     sum -= tmp[4];
00048     sum += y;
00049     tmp[4] = tmp[3];
00050     tmp[3] = tmp[2];
00051     tmp[2] = tmp[1];
00052     tmp[1] = tmp[0];
00053     tmp[0] = y;
00054     
00055     return sum/5;
00056 }
00057 
00058 
00059 void PidUpdate()
00060 {   
00061     static uint8_t Fflag = 0;
00062     
00063     pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0); 
00064     inputPID = (((int)(compass.sample() - ((standard) * 10.0) + 5400.0) % 3600) / 10.0);        
00065     
00066     //pc.printf("%f\n",timer1.read());
00067     pid.setProcessValue(inputPID);
00068     //timer1.reset();
00069     
00070     compassPID = -(pid.compute());
00071     
00072     if(!Fflag){
00073         Fflag = 1;
00074         compassPID = 0;
00075     }
00076     //pc.printf("%d\t%d\t%d\n",speed[0],speed[1],speed[2]);
00077     //pc.printf("standard = \t\t%f\n",standard);
00078     //pc.printf("%d\n",diff);
00079     //pc.printf("compass.sample = \t%f\n",compass.sample() / 1.0);
00080     //pc.printf("compassPID = \t\t%d\n",(int)compassPID);
00081     //pc.printf("inputPID = \t\t%f\n\n",inputPID);
00082     
00083     //pc.printf("%d\t%d\n",Distance,direction);
00084     //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
00085 }
00086 
00087 void IrDistanceUpdate()
00088 {
00089     for(uint8_t i = 0;i < 6;i++)
00090     {
00091         AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
00092         irDistance[i] = adcIn.read_u16() >> 4;
00093         //pc.printf("%d\n",irDistance[0]);
00094     }
00095 }
00096 
00097 /*
00098 void move(int vxx, int vyy, int vss)
00099 {
00100     double motVal[MOT_NUM] = {0};
00101     
00102     motVal[0] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
00103     motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
00104     motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
00105     motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
00106     
00107     for(uint8_t i = 0 ; i < MOT_NUM ; i++){
00108         if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
00109         else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
00110         speed[i] = (int)motVal[i];
00111     }
00112 }*/
00113 
00114 void move(int vxx, int vyy, int vss)
00115 {
00116     double motVal[MOT_NUM] = {0};
00117     
00118     int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
00119     
00120     double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
00121     
00122     if(angle == 180){
00123         hosei1 = 1.3;
00124     }
00125     
00126     motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
00127     motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2)*hosei2;
00128     motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
00129     motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4)*hosei4;
00130     
00131     for(uint8_t i = 0 ; i < MOT_NUM ; i++){
00132         if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
00133         else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
00134         speed[i] = motVal[i];
00135     }
00136     
00137     //pc.printf("speed1 = %d\n",speed[0]);
00138     //pc.printf("speed2 = %d\n",speed[1]);
00139     //pc.printf("speed3 = %d\n",speed[2]);
00140     //pc.printf("speed4 = %d\n\n",speed[3]);      
00141     
00142     ////pc.printf("%s",StringFIN.c_str());
00143 }
00144 
00145 int vector(double Amp,double degree,int xy)
00146 {
00147     double result; 
00148 
00149     if(xy == X){
00150         result = Amp * cos(degree * PI / 180.0);
00151     }else if(xy == Y){
00152         result = Amp * sin(degree * PI / 180.0) * (2.0 / sqrt(3.0));
00153     }
00154     
00155     return (int)result;
00156 }
00157 
00158 /***********  Serial interrupt  ***********/
00159 
00160 void Tx_interrupt()
00161 {
00162     array(speed[0],speed[1],speed[2],speed[3]);
00163     driver.printf("%s",StringFIN.c_str());
00164     //pc.printf("%s",StringFIN.c_str());
00165 }
00166 /*
00167 void Rx_interrupt()
00168 {
00169     if(driver.readable()){
00170         //pc.printf("%d\n",driver.getc());
00171     }
00172 }*/
00173 
00174 
00175 /***********  Serial interrupt **********/
00176 
00177 
00178 void init()
00179 {
00180     int scanfSuccess;
00181     int printfSuccess;
00182     int closeSuccess;
00183     int close2Success;
00184     uint8_t MissFlag = 0;
00185     FILE *fp;
00186     
00187     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00188     
00189     StartButton.mode(PullUp);
00190     CalibEnterButton.mode(PullUp);
00191     CalibExitButton.mode(PullUp);
00192     EEPROMButton.mode(PullUp);
00193     
00194     driver.baud(BAUD_RATE);
00195     device2.baud(BAUD_RATE2);
00196     wait_ms(MOTDRIVER_WAIT);
00197     driver.printf("1F0002F0003F0004F000\r\n"); 
00198     device2.printf("START");
00199     
00200     driver.attach(&Tx_interrupt, Serial::TxIrq);
00201     //driver.attach(&Rx_interrupt, Serial::RxIrq);
00202     device2.attach(&dev_rx,Serial::RxIrq);
00203     device2.attach(&dev_tx,Serial::TxIrq);
00204     underIR.attach(&whiteliner, 0.05);
00205     
00206     pid.setInputLimits(MINIMUM,MAXIMUM);
00207     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
00208     pid.setBias(PID_BIAS);
00209     pid.setMode(AUTO_MODE);
00210     pid.setSetPoint(REFERENCE);
00211     
00212     irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
00213     
00214     Survtimer.start();
00215     
00216     mbedleds = 1;
00217     
00218     while(StartButton){
00219         MissFlag = 0;
00220         if(!CalibEnterButton){
00221             mbedleds = 2;
00222             compass.setCalibrationMode(ENTER);
00223             while(CalibExitButton);
00224             compass.setCalibrationMode(EXIT);
00225             wait(BUT_WAIT);
00226             mbedleds = 4;
00227          }
00228          
00229          if(!EEPROMButton){
00230             Survtimer.reset();
00231             fp = fopen("/local/out.txt", "r");
00232             if(fp == NULL){
00233                 wait(BUT_WAIT);
00234                 MissFlag = 1;
00235             }else{
00236                 scanfSuccess = fscanf(fp, "%lf",&standard);
00237                 if(scanfSuccess == EOF){
00238                     wait(BUT_WAIT);
00239                     MissFlag = 1;
00240                 }else{
00241                     closeSuccess = fclose(fp);
00242                     if(closeSuccess == EOF){
00243                         wait(BUT_WAIT);
00244                         MissFlag = 1;
00245                     }else{
00246                         wait(BUT_WAIT);
00247                     }
00248                 }
00249             }
00250             if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){
00251                 for(uint8_t i = 0;i < 2;i++){
00252                     mbedleds = 15;
00253                     wait(0.1);
00254                     mbedleds = 0;
00255                     wait(0.1);
00256                 }
00257                 mbedleds = 15;
00258             }else{
00259                 mbedleds = 8;
00260             }
00261          }
00262          
00263          if(!CalibExitButton){
00264             Survtimer.reset();
00265          
00266             standard = compass.sample() / 10.0;
00267             
00268             fp = fopen("/local/out.txt", "w");
00269             if(fp == NULL){
00270                 wait(BUT_WAIT);
00271                 MissFlag = 1;
00272             }else{
00273                 printfSuccess = fprintf(fp, "%f",standard);
00274                 if(printfSuccess == EOF){
00275                     wait(BUT_WAIT);
00276                     MissFlag = 1;
00277                 }else{
00278                     close2Success = fclose(fp);
00279                     if(close2Success == EOF){
00280                         wait(BUT_WAIT);
00281                         MissFlag = 1;
00282                     }else{
00283                         wait(BUT_WAIT);
00284                     }
00285                 }
00286             }
00287             if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){
00288                 for(uint8_t i = 0;i < 4;i++){
00289                     mbedleds = 15;
00290                     wait(0.1);
00291                     mbedleds = 0;
00292                     wait(0.1);
00293                 }
00294                 mbedleds = 15;
00295             }else{
00296                 mbedleds = 10;
00297             }
00298         }
00299     }
00300     
00301     mbedleds = 0;
00302     
00303     Survtimer.stop();
00304     
00305     for(uint8_t i = 0;i < 6;i++)
00306     {
00307         stand[i] = irDistance[i];
00308     }
00309     irDistanceUpdata.detach();
00310     
00311     pidUpdata.attach(&PidUpdate, PID_CYCLE);
00312     //irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
00313     //timer1.start();
00314 }
00315 
00316 
00317 int main()
00318 {
00319     int vx=0,vy=0,vs=0;
00320     int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
00321     int state_off = NONE;
00322     int direction_av = 0;
00323     int direction_past = 0;
00324     int past_state_off = 0;
00325     int accelera_Distance = 0;
00326     uint8_t whiteFlag = 0;
00327     int save_vx = 0,save_vy = 0;
00328     
00329     int movein = 0;
00330     
00331     init();
00332            
00333     while(1) {
00334         
00335         vx=0;
00336         vy=0;
00337         
00338         //tuning = 0;
00339     
00340         x_dista = 0;
00341         y_dista = 0;
00342         x_turn  = 0;
00343         y_turn  = 0;
00344         //turn_flag = 0;
00345         
00346         vs = compassPID;
00347         
00348         //direction_av = moving_ave_5point(direction);
00349         
00350         
00351         
00352         for(uint8_t i = 0;i < 6;i++)
00353         {
00354             AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
00355             irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
00356             if(irDistance[i] >= 30)
00357             {
00358                 whiteFlag = 1;
00359                 movein = 1;
00360                 whiteout = 10;
00361             }        
00362             
00363             //pc.printf("%d\n",irDistance[0]);
00364         }       
00365         
00366         if(lineStateX == XNORMAL){
00367             if((LEFT_SONIC < 350) && (whiteFlag)){
00368                 lineStateX = LEFT_OUT;
00369             }
00370             if((LEFT_SONIC < 350) && (RIGHT_SONIC < 100) && (whiteFlag)){
00371                 lineStateX = LEFT_OUT;
00372             }
00373             
00374             if((RIGHT_SONIC < 350) && (whiteFlag)){    
00375                 lineStateX = RIGHT_OUT;         
00376             }
00377             if((RIGHT_SONIC < 350) && (LEFT_SONIC < 100) && (whiteFlag)){
00378                 lineStateX = RIGHT_OUT;
00379             }
00380             
00381         }else if(lineStateX == LEFT_OUT){
00382             /*
00383             if((LEFT_SONIC > 450) && (!whiteFlag)){
00384                 lineStateX = XNORMAL;
00385                 whiteFlag = 0;
00386             }
00387             */
00388             if((LEFT_SONIC > 450)){
00389                 lineStateX = XNORMAL;
00390                 whiteFlag = 0;
00391             }
00392         }else if(lineStateX == RIGHT_OUT){
00393             /*
00394             if((RIGHT_SONIC > 450) && (!whiteFlag)){
00395                 lineStateX = XNORMAL;
00396                 whiteFlag = 0;
00397             }
00398             */
00399             if((RIGHT_SONIC > 450)){
00400                 lineStateX = XNORMAL;
00401                 whiteFlag = 0;
00402             }
00403         }
00404         
00405         
00406         if(lineStateY == YNORMAL){
00407             if((FRONT_SONIC < 400) && (whiteFlag)){
00408                 lineStateY = FRONT_OUT;
00409             }
00410             if((FRONT_SONIC < 400)&& (BACK_SONIC < 100) && (whiteFlag)){
00411                 lineStateY = FRONT_OUT;
00412             }
00413             if((BACK_SONIC < 400) && (whiteFlag)){
00414                 lineStateY = BACK_OUT;
00415             }
00416             if((BACK_SONIC < 400) && (FRONT_SONIC < 100) && (whiteFlag)){
00417                 lineStateY = BACK_OUT;
00418             }
00419         }else if(lineStateY == FRONT_OUT){
00420             /*
00421             if((FRONT_SONIC > 500) && (!whiteFlag)){
00422                 lineStateY = YNORMAL;
00423                 whiteFlag = 0;
00424             }
00425             */
00426             if((FRONT_SONIC > 500)){
00427                 lineStateY = YNORMAL;
00428                 whiteFlag = 0;
00429             }
00430         }else if(lineStateY == BACK_OUT){
00431             /*
00432             if((BACK_SONIC > 500) && (!whiteFlag)){
00433                 lineStateY = YNORMAL;
00434                 whiteFlag = 0;
00435             }
00436             */
00437             if((BACK_SONIC > 500)){
00438                 lineStateY = YNORMAL;
00439                 whiteFlag = 0;
00440             }
00441         }
00442         
00443         
00444         if((state_off == ATTACK)&&(Distance == 10)){
00445             state = 1;
00446         }else{
00447             state = 0;
00448         }
00449            
00450         if(((direction == 0)||(direction == 1)||(direction == 15))){
00451             state_off = ATTACK;
00452         }
00453         if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 90)){
00454             if((past_state_off != SNAKE)){
00455                 accelera_Distance = Distance;    
00456             }
00457             state_off = SNAKE;
00458         }
00459         if(Distance >= 120){
00460             state_off = SEARCH;
00461         } 
00462         
00463         past_state_off = state_off;
00464         
00465         if(IR_found){
00466             if(state_off == SNAKE){
00467                 if((Distance == 30)||(Distance == 90)){
00468                     x_dista = 12*ball_sankaku[direction][0];
00469                     y_dista = 12*ball_sankaku[direction][1];            
00470                     
00471                     x_turn = 18*(turn_sankaku[direction][0]);
00472                     y_turn = 18*(turn_sankaku[direction][1]);
00473                     
00474                     if((direction == 2)||(direction == 14)){
00475                         //x_turn *= 0.7;
00476                         y_turn *= 0.7;
00477                     }
00478                     
00479                     if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
00480                         x_turn = 7*(turn_sankaku[direction][0]);
00481                         y_turn = 7*(turn_sankaku[direction][1]);
00482                         
00483                         x_dista = 15*ball_sankaku[direction][0];
00484                         y_dista = 10*ball_sankaku[direction][1];        
00485                     
00486                     }
00487                     
00488                     if(direction == 1){
00489                         vx = 15;
00490                         vy = 0;
00491                     }
00492                     
00493                     if(direction == 15){
00494                         vx = -15;
00495                         vy = 0;
00496                     }
00497                     
00498                     if(direction == 2){
00499                         vx = 20;
00500                         vy = -10;
00501                     }
00502                     
00503                     if(direction == 14){
00504                         vx = -20;
00505                         vy = -10;
00506                     }
00507                     
00508                 }
00509                 
00510                 if(Distance == 10){
00511                     x_dista = 8*(-ball_sankaku[direction][0]);
00512                     y_dista = 8*(-ball_sankaku[direction][1]);            
00513                 
00514                     x_turn = 22*(turn_sankaku[direction][0]);
00515                     y_turn = 22*(turn_sankaku[direction][1]);
00516                     
00517                     
00518                     if((direction == 2)||(direction == 14)){
00519                         y_turn *= 0.7;
00520                     }
00521                     
00522                     if(direction == 2){
00523                         vx = 20;
00524                         vy = -15;  
00525                     }
00526                     if(direction == 14){
00527                         vx = -20;
00528                         vy = -15;
00529                     }         
00530                 }
00531                 
00532                 if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
00533                     x_dista = 0;
00534                     y_dista = 0;    
00535                 }
00536                 
00537                 if(direction == 2){
00538                     vx = 20;
00539                     vy = -15;  
00540                 }
00541                 if(direction == 14){
00542                     vx = -20;
00543                     vy = -15;
00544                 }         
00545                     
00546                 vx = x_turn + x_dista;
00547                 vy = y_turn + y_dista;
00548                 
00549                 /*
00550                 if((accelera_Distance == 90)){
00551                     if(Distance == 10){
00552                         vx *= 0.3;
00553                         vy *= 0.3;
00554                     }    
00555                     
00556                     if(Distance == 30){
00557                         vx *= 0.4;
00558                         vy *= 0.4;
00559                     }  
00560                 }*/
00561                 /*
00562                 if((accelera_Distance == 10)){
00563                     if((direction == 4)||(direction == 12)){
00564                         vx = 0;
00565                         vy = -10;
00566                     }   
00567                 }*/
00568                 
00569             }else if(state_off == ATTACK){
00570                 if(direction == 0){
00571                     vx = 10*ball_sankaku[direction][0];
00572                     vy =20*ball_sankaku[direction][1];
00573                     /*
00574                     if(ultrasonicVal[1] < 380){
00575                         vy = 10;
00576                         vx = -20;
00577                     }  
00578                     
00579                     if(ultrasonicVal[3] < 380){
00580                         vy = 10;
00581                         vx = 20;
00582                     } */    
00583                 }
00584                 if(direction == 1){
00585                     vx = 15*1.3;
00586                     vy = 20*1.3;
00587                 }
00588                 if(direction == 15){
00589                     vx = -15*1.3;
00590                     vy = 20*1.3;  
00591                 }
00592                 if(direction == 2){
00593                     vx = 25;
00594                     vy = 0;
00595                 }
00596                 if(direction == 14){
00597                     vx = -25;
00598                     vy = 0;
00599                 }
00600                 
00601                 if(Distance > 30){
00602                     if(direction == 2){
00603                         vx = 20;
00604                         vy = 10;
00605                     }
00606                     if(direction == 14){
00607                         vx = -20;
00608                         vy = 10;
00609                     }    
00610                 }
00611                 
00612             }else if(state_off == SEARCH){
00613                 vx = 24*ball_sankaku[direction][0];
00614                 vy = 24*ball_sankaku[direction][1];
00615                 
00616                 if(direction == 2){
00617                     vx = 20;  
00618                 }
00619                 if(direction == 14){
00620                     vx = -20;
00621                 }          
00622                 if(direction == 0){
00623                     vx = 20*ball_sankaku[direction][0];
00624                     vy = 15*ball_sankaku[direction][1];  
00625                 }
00626                 if(direction == 1){
00627                     vx = 20*ball_sankaku[direction][0];
00628                     vy = 10*ball_sankaku[direction][1];  
00629                 }
00630                 if(direction == 15){
00631                     vx = 20*ball_sankaku[direction][0];
00632                     vy = 10*ball_sankaku[direction][1];  
00633                 }
00634                 if(direction == 4){
00635                     vx = 0;
00636                     vy = -15;
00637                 }
00638                 if(direction == 12){
00639                     vx = 0;
00640                     vy = -15;
00641                 }                         
00642             }
00643             
00644             if(direction == 2){
00645                 vx = 20;
00646                 vy = 0;
00647                   
00648             }
00649             if(direction == 14){
00650                 vx = -20;
00651                 vy = 0;
00652             } 
00653              
00654         }else{
00655             vx = 0;
00656             vy = 0;
00657         }
00658         
00659         vx *= 1.3* 0.8;
00660         vy *= 0.7 * 0.8;
00661         
00662 
00663          
00664         if(lineStateX == LEFT_OUT){    
00665             vx += 20;
00666         }else if(lineStateX == RIGHT_OUT){
00667             vx -= 20;
00668         }
00669         
00670         if(lineStateY == FRONT_OUT){
00671             vy -= 15;
00672         }else if(lineStateY == BACK_OUT){
00673             vy += 15;
00674         } 
00675         
00676         //vx *= 0.8;
00677         //vy *= 0.8;
00678         
00679         direction_past = direction;
00680     
00681         
00682         move(vx,vy,vs);
00683     }
00684 }