main.cpp

00001 #include <mbed.h>
00002 #include <math.h>
00003 #include <sstream>
00004 #include "mbed.h"
00005 #include "HMC6352.h"
00006 #include "PID.h"
00007 #include "main.h"
00008 #include "eeprom.h"
00009 
00010 
00011 BusOut mbedleds(LED4,LED3,LED2,LED1);
00012 
00013 void PidUpdata()
00014 {   
00015     if(turn_flag){
00016         now_compass = (((int)(compass.sample() - (past_compass) + 5400.0) % 3600) / 10.0); 
00017     }else{ 
00018         inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
00019     
00020     //pc.printf("%f\n",timer1.read());
00021         pid.setProcessValue(inputPID);
00022     //timer1.reset();
00023     
00024         compassPID = -(pid.compute());
00025     }
00026     //pc.printf("%f\n",compassPID);
00027 
00028 }
00029 
00030 void move(int vxx, int vyy, int vss)
00031 {
00032     double motVal[MOT_NUM] = {0};
00033     
00034     int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
00035     
00036     double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
00037     
00038     if((angle > 30)&&(angle < 80)){
00039         hosei2 = 0.7;
00040         hosei4 = 0.7;
00041     }
00042     
00043     motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
00044     motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2)*hosei2;
00045     motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
00046     motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4)*hosei4;
00047     
00048     for(uint8_t i = 0 ; i < MOT_NUM ; i++){
00049         if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
00050         else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
00051         speed[i] = motVal[i];
00052     }
00053     /*
00054     pc.printf("speed1 = %d\n",speed[0]);
00055     pc.printf("speed2 = %d\n",speed[1]);
00056     pc.printf("speed3 = %d\n",speed[2]);
00057     pc.printf("speed4 = %d\n\n",speed[3]);      
00058     */
00059     ////pc.printf("%s",StringFIN.c_str());
00060 }
00061 
00062 /***********  Serial interrupt  ***********/
00063 
00064 void Tx_interrupt()
00065 {
00066     array(speed[0],speed[1],speed[2],speed[3]);
00067     driver.printf("%s",StringFIN.c_str());
00068     //pc.printf("%s",StringFIN.c_str());
00069     //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
00070 }
00071 /*
00072 void Rx_interrupt()
00073 {
00074     if(driver.readable()){
00075         //pc.printf("%d\n",driver.getc());
00076     }
00077 }*/
00078 
00079 
00080 /***********  Serial interrupt end **********/
00081 
00082 
00083 void init()
00084 {
00085     uint8_t initFlag = 0;
00086     char *hozon;
00087     
00088     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00089     StartButton.mode(PullUp);
00090     CalibEnterButton.mode(PullUp);
00091     CalibExitButton.mode(PullUp);
00092     EEPROMButton.mode(PullUp);
00093     driver.baud(BAUD_RATE);
00094     wait_ms(MOTDRIVER_WAIT);
00095     driver.printf("1F0002F0003F0004F000\r\n"); 
00096     
00097     led1 = ON;
00098     
00099     while(StartButton){
00100         if(!CalibEnterButton){
00101             led1 = OFF;
00102             led2 = ON;
00103             compass.setCalibrationMode(ENTER);
00104             while(CalibExitButton);
00105             compass.setCalibrationMode(EXIT);
00106             led2 = OFF;
00107             led3 = ON;
00108          }
00109          if(!EEPROMButton){
00110             initFlag = 1;
00111             read_eeprom(hozon,(char *)&standard,sizeof(hozon));
00112          }
00113     }
00114     if(!initFlag){
00115         standard = compass.sample() / 10.0;
00116         write_eeprom((char *)&standard,hozon,sizeof((char *)&standard));
00117     }
00118     led1 = OFF;
00119     led3 = OFF;
00120     
00121     pid.setInputLimits(0.0, 360.0);
00122     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
00123     pid.setBias(0.0);
00124     pid.setMode(AUTO_MODE);
00125     pid.setSetPoint(180.0);
00126     
00127     pidUpdata.attach(&PidUpdata, 0.06);
00128     wait(0.3);
00129     IR.attach(&IR_Position,0.06);
00130     ultrasonic.attach(&Ultrasonic, 0.05);
00131     driver.attach(&Tx_interrupt, Serial::TxIrq);
00132     //driver.attach(&Rx_interrupt, Serial::RxIrq);
00133     
00134     timer1.start();
00135     timer2.start();
00136 }
00137 
00138 uint16_t moving_ave_5point(uint16_t data)
00139 {
00140     static uint16_t tmp[5] = {0};
00141     static uint32_t sum = 0;
00142     uint8_t i;
00143     uint8_t count;
00144     
00145     sum -= tmp[4];
00146     sum += data;
00147     tmp[4] = tmp[3];
00148     tmp[3] = tmp[2];
00149     tmp[2] = tmp[1];
00150     tmp[1] = tmp[0];
00151     tmp[0] = data;
00152     
00153     return sum/5;
00154 }
00155 
00156 
00157 int main()
00158 {
00159     int vx=0,vy=0,vs=0;
00160     int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
00161     int state = NONE;
00162     int direction_av = 0;
00163     int direction_past = 0;
00164     
00165     init();
00166            
00167     while(1) {
00168         x_dista = 0;
00169         y_dista = 0;
00170         x_turn  = 0;
00171         y_turn  = 0;
00172         turn_flag = 0;
00173         
00174         vs = compassPID;
00175         //vs = 0;
00176         //past_compass = compass.sample() / 1.0;
00177         //float now_compass = 180.0;
00178         /*
00179         while(1){
00180             vx = 10;
00181             vy = 10;
00182             vs = compassPID;
00183             
00184             move(vx,vy,vs);
00185         }*/
00186         
00187         direction_av = moving_ave_5point(direction);
00188            
00189         if(direction_av == 0){
00190             state = ATTACK;
00191         }
00192         if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 30)){
00193             state = SNAKE;
00194         }
00195         if(Distance >= 90){
00196             state = SEARCH;
00197         } 
00198         
00199         if(IR_found){
00200             if(state == SNAKE){
00201                 if(Distance == 30){
00202                     x_dista = 20*ball_sankaku[direction][0];
00203                     y_dista = 20*ball_sankaku[direction][1];            
00204                     
00205                     x_turn = 10*(turn_sankaku[direction][0]);
00206                     y_turn = 10*(turn_sankaku[direction][1]);
00207                     
00208                     if((direction == 2)||(direction == 14)){
00209                         x_turn *= 0.7;
00210                         y_turn *= 0.7;
00211                     }
00212                 }
00213                 
00214                 if(Distance == 10){
00215                     x_dista = 8*(-ball_sankaku[direction][0]);
00216                     y_dista = 8*(-ball_sankaku[direction][1]);            
00217                 
00218                     x_turn = 22*(turn_sankaku[direction][0]);
00219                     y_turn = 22*(turn_sankaku[direction][1]);
00220                     
00221                     
00222                     if(direction == 2){
00223                         vs = -3;  
00224                     }
00225                     if(direction == 14){
00226                         vs = 3;  
00227                     }
00228                     
00229                     if((direction == 2)||(direction == 14)){
00230                         x_turn *= 0.7;
00231                         y_turn *= 0.7;
00232                     }
00233                 }
00234                 
00235                 if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
00236                     x_dista = 0;
00237                     y_dista = 0;    
00238                 }
00239                     
00240                 vx = x_turn + x_dista;
00241                 vy = y_turn + y_dista;
00242                 
00243             }else if(state == ATTACK){
00244                 if(direction == 0){
00245                     vx = 10*ball_sankaku[direction][0];
00246                     vy = 15*ball_sankaku[direction][1];  
00247                 }
00248                 if(direction == 1){
00249                     vx = 20*ball_sankaku[direction][0];
00250                     vy = 12*ball_sankaku[direction][1];
00251                     vs = -2;  
00252                 }
00253                 if(direction == 15){
00254                     vx = 20*ball_sankaku[direction][0];
00255                     vy = 12*ball_sankaku[direction][1];
00256                     vs = 2;  
00257                 }
00258                 if(direction == 2){
00259                     vx = 10*ball_sankaku[direction][0];
00260                     vy = 10*ball_sankaku[direction][1];
00261                 }
00262                 if(direction == 14){
00263                     vx = 10*ball_sankaku[direction][0];
00264                     vy = 10*ball_sankaku[direction][1];
00265                 }
00266                 
00267             }else if(state == SEARCH){
00268                 vx = 15*ball_sankaku[direction][0];
00269                 vy = 15*ball_sankaku[direction][1];
00270                 
00271                 if(direction == 2){
00272                     vx = 25*ball_sankaku[direction][0];
00273                     vy = 10*ball_sankaku[direction][1];
00274                     vs = -2;  
00275                 }
00276                 if(direction == 14){
00277                     vx = -25*ball_sankaku[direction][0];
00278                     vy = 10*ball_sankaku[direction][1];
00279                     //vs = 2;  
00280                 }          
00281                 if(direction == 0){
00282                     vx = 10*ball_sankaku[direction][0];
00283                     vy = 15*ball_sankaku[direction][1];  
00284                 }
00285                 if(direction == 1){
00286                     vx = 30*ball_sankaku[direction][0];
00287                     vy = 8*ball_sankaku[direction][1];  
00288                 }
00289                 if(direction == 15){
00290                     vx = 30*ball_sankaku[direction][0];
00291                     vy = 8*ball_sankaku[direction][1];  
00292                 }               
00293             } 
00294         }else{
00295             vx = 0;
00296             vy = 0;
00297         }
00298         
00299         /*
00300         if((ultrasonicVal[0]<50)||(ultrasonicVal[1]<50)||(ultrasonicVal[3]<50)){
00301             vx = (int)((30-FULL)*ball_sankaku[direction][0]);
00302             vy = (int)((30-FULL)*ball_sankaku[direction][1]);
00303         }
00304         
00305         
00306         
00307         if(Distance == 10){
00308             mbedleds = 0xF;    
00309         }else{
00310             mbedleds = 0x0;
00311         }
00312         
00313         if((ultrasonicVal[0]<100)&&(ultrasonicVal[1]<100)&&(ultrasonicVal[2]<100)&&((direction == 0)||(direction == 1)||(direction == 15))){
00314             vx = 0;
00315             vy = 0;
00316             vs = 0;
00317             past_compass = compass.sample() / 1.0;
00318             float now_compass = 180.0;
00319             
00320             if(inputPID > 180){
00321                 turn_flag = 1;
00322                 while((now_compass > 180.0 - (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
00323                     vs = 10;
00324                     move(vx,vy,vs);
00325                 }
00326                 turn_flag = 0;
00327             }
00328             
00329             if(inputPID < 180){
00330                 turn_flag = 1;
00331                 while((now_compass < 180.0 + (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
00332                     vs = -10;
00333                     move(vx,vy,vs);
00334                 }
00335                 turn_flag = 0; 
00336             }          
00337         }*/
00338         
00339         if(state == SNAKE){
00340             mbedleds = 0xF;    
00341         }else{
00342             mbedleds = 0x0;
00343         }
00344                 
00345         
00346         vx *= 1;
00347         vy *= 1;
00348         
00349         direction_past = direction;
00350         
00351         move(vx,vy,vs);
00352     }
00353 }