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