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 
00010 void PidUpdate()
00011 {   
00012     static uint8_t Fflag = 0;
00013     
00014     inputPID = (((int)(compass.sample() - ((standard + standTu) * 10.0) + 5400.0) % 3600) / 10.0);        
00015     
00016     //pc.printf("%f\n",timer1.read());
00017     pid.setProcessValue(inputPID);
00018     //timer1.reset();
00019     
00020     compassPID = -(pid.compute());
00021     
00022     if(!Fflag){
00023         Fflag = 1;
00024         compassPID = 0;
00025     }
00026     //pc.printf("%f\n",standard);
00027     //pc.printf("%d\n",diff);
00028     //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
00029     //pc.printf("%d\n",(int)compassPID);
00030     //pc.printf("%d\t%d\n",Distance,direction);
00031     //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
00032 }
00033 
00034 void move(int vxx, int vyy, int vss)
00035 {
00036     double motVal[MOT_NUM] = {0};
00037     
00038     motVal[0] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
00039     motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
00040     motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
00041     motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
00042     
00043     for(uint8_t i = 0 ; i < MOT_NUM ; i++){
00044         if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
00045         else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
00046         speed[i] = (int)motVal[i];
00047     }
00048 }
00049 
00050 /***********  Serial interrupt  ***********/
00051 
00052 void Tx_interrupt()
00053 {
00054     array(speed[0],speed[1],speed[2],speed[3]);
00055     driver.printf("%s",StringFIN.c_str());
00056     //pc.printf("%s",StringFIN.c_str());
00057 }
00058 /*
00059 void Rx_interrupt()
00060 {
00061     if(driver.readable()){
00062         //pc.printf("%d\n",driver.getc());
00063     }
00064 }*/
00065 
00066 
00067 /***********  Serial interrupt **********/
00068 
00069 
00070 void init()
00071 {
00072     int scanfSuccess;
00073     int printfSuccess;
00074     int closeSuccess;
00075     int close2Success;
00076     FILE *fp;
00077     
00078     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00079     
00080     StartButton.mode(PullUp);
00081     CalibEnterButton.mode(PullUp);
00082     CalibExitButton.mode(PullUp);
00083     EEPROMButton.mode(PullUp);
00084     
00085     driver.baud(BAUD_RATE);
00086     device2.baud(BAUD_RATE2);
00087     wait_ms(MOTDRIVER_WAIT);///////////////////////////////////////////////////////////////
00088     driver.printf("1F0002F0003F0004F000\r\n"); 
00089     device2.printf("START");
00090     
00091     driver.attach(&Tx_interrupt, Serial::TxIrq);
00092     //driver.attach(&Rx_interrupt, Serial::RxIrq);
00093     device2.attach(&dev_rx,Serial::RxIrq);
00094     device2.attach(&dev_tx,Serial::TxIrq);
00095     
00096     mbedleds = 1;
00097     
00098     while(StartButton){
00099         if(!CalibEnterButton){
00100             mbedleds = 2;
00101             compass.setCalibrationMode(ENTER);
00102             while(CalibExitButton);
00103             compass.setCalibrationMode(EXIT);
00104             wait(BUT_WAIT);
00105             mbedleds = 4;
00106          }
00107          
00108          if(!EEPROMButton){
00109             fp = fopen("/local/out.txt", "r");
00110             if(fp == NULL){
00111                 wait(BUT_WAIT);
00112                 mbedleds = 3;
00113             }else{
00114                 scanfSuccess = fscanf(fp, "%lf",&standard);
00115                 if(scanfSuccess == EOF){
00116                     wait(BUT_WAIT);
00117                     mbedleds = 7;
00118                 }else{
00119                     closeSuccess = fclose(fp);
00120                     if(closeSuccess == EOF){
00121                         wait(BUT_WAIT);
00122                         mbedleds = 15;
00123                     }else{
00124                         wait(BUT_WAIT);
00125                         mbedleds = 8;
00126                     }
00127                 }
00128              }
00129          }
00130          
00131          if(!CalibExitButton){
00132             standard = compass.sample() / 10.0;
00133             
00134             fp = fopen("/local/out.txt", "w");
00135             if(fp == NULL){
00136                 wait(BUT_WAIT);
00137                 mbedleds = 3;
00138             }else{
00139                 printfSuccess = fprintf(fp, "%f",standard);
00140                 if(printfSuccess == EOF){
00141                     wait(BUT_WAIT);
00142                     mbedleds = 7;
00143                 }else{
00144                     close2Success = fclose(fp);
00145                     if(close2Success == EOF){
00146                         wait(BUT_WAIT);
00147                         mbedleds = 15;
00148                     }else{
00149                         wait(BUT_WAIT);
00150                         mbedleds = 4;
00151                     }
00152                 }
00153              }
00154         }
00155     }
00156     
00157     mbedleds = 0;
00158     
00159     pid.setInputLimits(0.0, 360.0);
00160     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
00161     pid.setBias(0.0);
00162     pid.setMode(AUTO_MODE);
00163     pid.setSetPoint(180.0);
00164     
00165     pidUpdata.attach(&PidUpdate, 0.06);
00166     
00167     //timer1.start();
00168     //Ctimer.start();
00169 }
00170 
00171 int main()
00172 {
00173     int vx=0,vy=0,vs=0;
00174     
00175     init();
00176            
00177     while(1){
00178         
00179         hold_flag = 0;
00180         vx = 0;
00181         vy = 0;
00182         vs = (int)compassPID;
00183         
00184         
00185         /*************** Change state **************************/
00186         
00187         if(state == HOLD){
00188             if(FRONT_SONIC < 100)hold_flag = 1;
00189             
00190             if(Distance > 140){
00191                 mbedleds = 1;
00192                 state = HOME_WAIT;
00193             }else if(!((direction == 0) || (direction == 15) || (direction == 1))){
00194                 mbedleds = 4;
00195                 state = DIFFENCE;
00196             }else if(!(Distance <= 30)){
00197                 mbedleds = 4;
00198                 state = DIFFENCE;
00199             }
00200         }else{
00201             standTu = 0;
00202             if(state == DIFFENCE){
00203                 
00204                 if((direction == 0) && (Distance <= 10) && (IR_found)){
00205                     mbedleds = 8;
00206                     state = HOLD;
00207                 }else if((Distance <= 140) && (IR_found)){
00208                     mbedleds = 4;
00209                     state = DIFFENCE;
00210                 }else{
00211                     if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
00212                         if(IR_found){
00213                             mbedleds = 4;
00214                             state = DIFFENCE;
00215                         }
00216                     }else if(diff > 15){
00217                          mbedleds = 4;
00218                          state = DIFFENCE;
00219                     }else{
00220                         mbedleds = 1;
00221                         state = HOME_WAIT;
00222                     }
00223                 }
00224                 
00225             }else{  
00226                 if((direction == 0) && (Distance <= 10) && (IR_found)){
00227                     mbedleds = 8;
00228                     state = HOLD;
00229                 }else if((Distance <= 120) && (IR_found)){
00230                     mbedleds = 4;
00231                     state = DIFFENCE;
00232                 }else{
00233                     if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
00234                         if(IR_found){
00235                             mbedleds = 4;
00236                             state = DIFFENCE;
00237                         }       
00238                     }else if(diff > 15){
00239                          mbedleds = 4;
00240                          state = DIFFENCE;
00241                     }else if((Distance <= 120) && (IR_found)){
00242                         mbedleds = 2;
00243                         state = WARNING;
00244                     }else{
00245                         mbedleds = 1;
00246                         state = HOME_WAIT;
00247                     }
00248                 }
00249             }
00250         }
00251         /*************** Change state **************************/
00252         //pc.printf("%f\t%f\t%f\t%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
00253         
00254         if(state == HOME_WAIT){
00255             if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 900.0)){
00256                 if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
00257                     vx = 0;
00258                 }else if(RIGHT_SONIC < 450.0){
00259                     vx = (int)((RIGHT_SONIC - 450.0) * 0.01 - 10.0);
00260                     if(vx < -15)vx = -15;
00261                 }else if(LEFT_SONIC < 450.0){
00262                     vx = (int)((450.0 - LEFT_SONIC ) * 0.01 + 10.0);
00263                     if(vx > 15)vx = 15;
00264                 }
00265                 
00266                 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
00267                       if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
00268                         vy = 0;
00269                     }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
00270                         vy = 4;
00271                     }else{
00272                         vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
00273                         if(vy < -10)vy = -10;
00274                     }
00275                 }else{
00276                     if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
00277                         vy = 0;
00278                     }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
00279                         vy = 4;
00280                     }else{
00281                         vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
00282                         if(vy < -10)vy = -10;
00283                     }
00284                 }
00285             }else if((RIGHT_SONIC + LEFT_SONIC) <= 900.0){
00286                 if(BACK_SONIC < 100.0){
00287                     if(RIGHT_SONIC > LEFT_SONIC){
00288                         vx = 10;
00289                         vy = 0;
00290                     }else{
00291                         vx = -10;
00292                         vy = 0;
00293                     }
00294                 }else if(BACK_SONIC < 200.0){
00295                      if(RIGHT_SONIC > LEFT_SONIC){
00296                         vx = 10;
00297                         vy = -4;
00298                     }else{
00299                         vx = -10;
00300                         vy = -4;
00301                     }
00302                 }else{
00303                     vx = 0;
00304                     vy = -10;
00305                 }
00306             }else{
00307                 if(BACK_SONIC > 500.0){
00308                     if(RIGHT_SONIC > LEFT_SONIC){
00309                         vx = 10;
00310                         vy = -5;
00311                     }else{                    
00312                         vx = -10;
00313                         vy = -5;
00314                     }
00315                 }
00316             }
00317         }else if(state == DIFFENCE){
00318             if(direction == 6){
00319             
00320                 if(BACK_SONIC < 110.0){
00321                     vy = 4;
00322                 }else  if(BACK_SONIC > 500.0){
00323                     vy = -10;
00324                 }else if(BACK_SONIC > 300.0){
00325                     vy = -7;
00326                 }else if(BACK_SONIC > 140.0){
00327                     vy = -5;
00328                 }else{
00329                     vy = 0;
00330                 }
00331                      
00332                 if(LEFT_SONIC < 700.0){
00333                     vx = 15;
00334                 }else{
00335                     if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
00336                         vx = 10;
00337                         vy = -5;
00338                     }else{
00339                         vx = 10;
00340                         vy = -5;
00341                     }
00342                 }
00343                 
00344             }else if(direction == 10){
00345                 /*
00346                 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
00347                       if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
00348                         vy = 0;
00349                     }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
00350                         vy = 4;
00351                     }else{
00352                         vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
00353                         if(vy < -10)vy = -10;
00354                     }
00355                 }else{
00356                     if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
00357                         vy = 0;
00358                     }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
00359                         vy = 4;
00360                     }else{
00361                         vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
00362                         if(vy < -10)vy = -10;
00363                     }
00364                 }
00365                      
00366                 if(RIGHT_SONIC < 700.0){
00367                     vx = -15;
00368                 }else{
00369                     if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
00370                         vx = -10;
00371                         vy = -5;
00372                     }else{
00373                         vx = -10;
00374                         vy = -5;
00375                     }
00376                 }
00377                 */
00378                 if(BACK_SONIC < 110.0){
00379                     vy = 4;
00380                 }else  if(BACK_SONIC > 500.0){
00381                     vy = -10;
00382                 }else if(BACK_SONIC > 300.0){
00383                     vy = -7;
00384                 }else if(BACK_SONIC > 140.0){
00385                     vy = -5;
00386                 }else{
00387                     vy = 0;
00388                 }
00389                      
00390                 if(RIGHT_SONIC < 700.0){
00391                     vx = -15;
00392                 }else{
00393                     if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
00394                         vx = -10;
00395                         vy = -5;
00396                     }else{
00397                         vx = -10;
00398                         vy = -5;
00399                     }
00400                 }
00401             }else if(direction == 4){
00402             
00403                 if(BACK_SONIC < 110.0){
00404                     vy = 4;
00405                 }else  if(BACK_SONIC > 500.0){
00406                     vy = -10;
00407                 }else if(BACK_SONIC > 300.0){
00408                     vy = -7;
00409                 }else if(BACK_SONIC > 140.0){
00410                     vy = -5;
00411                 }else{
00412                     vy = 0;
00413                 }
00414                      
00415                 if(LEFT_SONIC < 700.0){
00416                     vx = 15;
00417                 }else{
00418                     if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
00419                         vx = 10;
00420                         vy = -5;
00421                     }else{
00422                         vx = 10;
00423                         vy = -5;
00424                     }
00425                 }
00426                 
00427             }else if(direction == 12){
00428             
00429                 if(BACK_SONIC < 110.0){
00430                     vy = 4;
00431                 }else  if(BACK_SONIC > 500.0){
00432                     vy = -10;
00433                 }else if(BACK_SONIC > 300.0){
00434                     vy = -7;
00435                 }else if(BACK_SONIC > 140.0){
00436                     vy = -5;
00437                 }else{
00438                     vy = 0;
00439                 }
00440                      
00441                 if(RIGHT_SONIC < 700.0){
00442                     vx = -15;
00443                 }else{
00444                     if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
00445                         vx = -10;
00446                         vy = -5;
00447                     }else{
00448                         vx = -10;
00449                         vy = -5;
00450                     }
00451                 }
00452                 
00453             }else if(direction == 8){
00454             
00455                 if(LEFT_SONIC > RIGHT_SONIC){
00456                     vx = -10;
00457                 }else{
00458                     vx = 10;
00459                 }
00460                 vy = -5;
00461                 
00462             }else if(direction == 2){
00463                 /*
00464                 if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
00465                     vx = 30;
00466                 }else{
00467                     vx = 20;
00468                 } 
00469                 */
00470                 vx = 15;
00471                 if(BACK_SONIC < 130.0){
00472                     //vy = 5;
00473                 }
00474                 /*
00475                 if(BACK_SONIC > 140.0){
00476                     vy = -10;
00477                 }else if(BACK_SONIC < 110.0){
00478                     vy = 5;
00479                 }
00480                 */
00481             }else if(direction == 14){
00482                 /*
00483                 if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
00484                     vx = -30;
00485                 }else{
00486                     vx = -20;
00487                 } 
00488                 */
00489                 vx = -15;   
00490                 
00491                 if(BACK_SONIC < 130.0){
00492                     //vy = 5;
00493                 }
00494                 
00495                 /*
00496                 if(BACK_SONIC > 140.0){
00497                     vy = -10;
00498                 }else if(BACK_SONIC < 110.0){
00499                     vy = 5;
00500                 }
00501                 */
00502             }else if(direction == 1){
00503             
00504                 vx = 10;    
00505                 
00506             }else if(direction == 15){
00507             
00508                 vx = -10;
00509                 
00510             }else if(direction == 0){
00511             
00512                 vx = 0;
00513                 vy = 10;
00514                 
00515             }else{//error
00516             
00517                 vx = 0;
00518                 vy = 0;
00519             
00520             }
00521         }else if(state == WARNING){
00522             if(direction == 0){
00523             
00524                 vx = 0;
00525                 
00526             }else if(direction == 1){
00527             
00528                 vx = 15;
00529                 
00530             }else if(direction == 2){
00531             
00532                 vx = 20;
00533                 
00534             }else if(direction == 14){
00535             
00536                 vx = -20;
00537                 
00538             }else if(direction == 15){
00539             
00540                 vx = -15;
00541                 
00542             }
00543             
00544             if((LEFT_SONIC > 320) && (RIGHT_SONIC > 320)){
00545                 if(BACK_SONIC > 150.0){
00546                     vy = -5;
00547                 }else if(BACK_SONIC < 110.0){
00548                     vy = 5;
00549                 }else{
00550                     vy = 0;
00551                 }
00552             }else{
00553                 if(BACK_SONIC > 110.0){
00554                     vy = -5;
00555                 }else if(BACK_SONIC < 70.0){
00556                     vy = 5;
00557                 }else{
00558                     vy = 0;
00559                 }
00560             }
00561         }else if(state == HOLD){
00562             vy = 10;
00563             if(FRONT_SONIC < 100){
00564                 if(RIGHT_SONIC < LEFT_SONIC){
00565                     if(BACK_SONIC > 500){
00566                         vy = 0;
00567                         vs = 3;
00568                     }
00569                 }else{
00570                     if(BACK_SONIC > 500){
00571                         vy = 0;
00572                         vs = -3;
00573                     }
00574                 }
00575             }else{
00576                 if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 800.0)){
00577                     standTu = (RIGHT_SONIC - LEFT_SONIC) / 40.0;
00578                 }else{
00579                     standTu = 0;
00580                 }
00581             }
00582         }
00583         //vx = (int)(vx * 0.8);
00584         move(vx,vy,vs);
00585     }
00586 }