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     
00020     //pc.printf("%f\n",compassPID);
00021 
00022 }
00023 
00024 void move(int vxx, int vyy, int vss)
00025 {
00026     double motVal[MOT_NUM] = {0};
00027     
00028     motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
00029     motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
00030     motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
00031     motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
00032     
00033     for(uint8_t i = 0 ; i < MOT_NUM ; i++){
00034         if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
00035         else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
00036         speed[i] = motVal[i];
00037     }
00038     /*
00039     pc.printf("speed1 = %d\n",speed[0]);
00040     pc.printf("speed2 = %d\n",speed[1]);
00041     pc.printf("speed3 = %d\n",speed[2]);
00042     pc.printf("speed4 = %d\n\n",speed[3]);      
00043     */
00044     ////pc.printf("%s",StringFIN.c_str());
00045 }
00046 
00047 /***********  Serial interrupt  ***********/
00048 
00049 void Tx_interrupt()
00050 {
00051     array(speed[0],speed[1],speed[2],speed[3]);
00052     driver.printf("%s",StringFIN.c_str());
00053     //pc.printf("%s",StringFIN.c_str());
00054     //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
00055 }
00056 /*
00057 void Rx_interrupt()
00058 {
00059     if(driver.readable()){
00060         //pc.printf("%d\n",driver.getc());
00061     }
00062 }*/
00063 
00064 
00065 /***********  Serial interrupt end **********/
00066 
00067 
00068 void init()
00069 {
00070     
00071     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00072     StartButton.mode(PullUp);
00073     CalibEnterButton.mode(PullUp);
00074     CalibExitButton.mode(PullUp);
00075     driver.baud(BAUD_RATE);
00076     wait_ms(MOTDRIVER_WAIT);
00077     driver.printf("1F0002F0003F0004F000\r\n"); 
00078     
00079     led1 = ON;
00080     
00081     while(StartButton){
00082         if(!CalibEnterButton){
00083             led1 = OFF;
00084             led2 = ON;
00085             compass.setCalibrationMode(ENTER);
00086             while(CalibExitButton);
00087             compass.setCalibrationMode(EXIT);
00088             led2 = OFF;
00089             led3 = ON;
00090          }
00091     }
00092     
00093     standard = compass.sample() / 10.0;
00094     
00095     led1 = OFF;
00096     led3 = OFF;
00097     
00098     pid.setInputLimits(0.0, 360.0);
00099     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
00100     pid.setBias(0.0);
00101     pid.setMode(AUTO_MODE);
00102     pid.setSetPoint(180.0);
00103     
00104     pidUpdata.attach(&PidUpdata, 0.06);
00105     wait(1);
00106     IR.attach(&IR_Position,0.04);
00107     ultrasonic.attach(&Ultrasonic, 0.05);
00108     driver.attach(&Tx_interrupt, Serial::TxIrq);
00109     //driver.attach(&Rx_interrupt, Serial::RxIrq);
00110     
00111     timer1.start();
00112     timer2.start();
00113 }
00114 
00115 int main()
00116 {
00117     int vx=0,vy=0,vs=0;
00118     int state = HOME_WAIT;
00119     
00120     init();
00121            
00122     while(1) {
00123     
00124         vs = compassPID;
00125         //vx = 15;
00126         //vy = 10;
00127         
00128         /*
00129         if(IR_found){
00130             if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
00131                 vx = (int)(70*ball_sankaku[direction][0]);
00132                 vy = (int)(70*ball_sankaku[direction][1]);
00133             }else{
00134                 vx = (int)(70*ball_sankaku[direction][0]);
00135                 vy = (int)(70*ball_sankaku[direction][1]);
00136             }
00137         }else{
00138             vx = 0;
00139             vy = 0;
00140         }
00141         */
00142         
00143         /*
00144         if(IR_found)state = DIFFENCE;
00145         else state = HOME_WAIT;
00146         */
00147         /*
00148         if(state == HOME_WAIT){
00149             if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
00150                 if(ultrasonicVal[0] > 3200){
00151                     vx = 15;
00152                     vy = 0;
00153                 }else if(ultrasonicVal[2] > 3200){
00154                     vx = -15;
00155                     vy = 0;
00156                 }else{
00157                     if(ultrasonicVal[1] > 800){
00158                         vx = 0;
00159                         vy = -15;
00160                     }else{
00161                         vx = 0;
00162                         vy = 0;
00163                     }   
00164                 }
00165             }else{
00166                 vx = 0;
00167                 vy = 0;
00168             }
00169         }else if(state == DIFFENCE){
00170             if(ultrasonicVal[1] > 800){
00171                 vx = 0;
00172                 vy = -15;
00173             }else{
00174                 if(distance <= 30){
00175                 
00176                     if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
00177                         vx = 15;
00178                 
00179                     }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
00180                         vx = -15;
00181             
00182                     }else{
00183                         vx = 0;
00184                     }
00185                 }else{
00186                     
00187                 }
00188             }     
00189         }
00190         */
00191         /*
00192         if(state == HOME_WAIT){
00193            
00194         }*/
00195         
00196             if((ultrasonicVal[0] + ultrasonicVal[2]) < 1050.0){
00197                 if(ultrasonicVal[0] > 300.0){
00198                     vx = 15;
00199                     led3 = ON;
00200                     led4 = OFF;
00201                 }else if(ultrasonicVal[2] > 300.0){
00202                     vx = -15;
00203                     led3 = ON;
00204                     led4 = OFF;
00205                 }else{
00206                     led3 = OFF;
00207                     led4 = ON;
00208                     if(ultrasonicVal[1] > 200.0){
00209                         vy = -15;
00210                     }else if(ultrasonicVal[1] < 100.0){
00211                         vy = 15;
00212                     }else{
00213                         vy = 0;
00214                     }
00215                 }
00216                 led2 = ON;
00217             }else{
00218                 vx = 0;
00219                 vy = 0;
00220                 led2 = OFF;
00221             }
00222             
00223         //pc.printf("%f,%f,%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2]);
00224             
00225         move(vx,vy,vs);
00226     }
00227 }