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