main.cpp

00001 #include<mbed.h>
00002 #include"MS5607I2C.h"
00003 #include"ADXL345_I2C.h"
00004 #include "SDFileSystem.h"
00005 #include "HMC5883L.h"
00006 #include<stdio.h>
00007 //#include<math.h>
00008 
00009 #define SDA      p9
00010 #define SCL      p10
00011 #define Rs       p21             //right servo motor
00012 #define Ls       p22             //left servo motor
00013 #define PI       3.14159265
00014 #define N_SAMPLE 256
00015 #define WAIT_TIME 50
00016 #define N_DIM 4
00017 #define EPS 1e-8
00018 #define goallat 4008.544783
00019 #define goallon 13959.2389
00020 
00021 SDFileSystem sd(p5, p6, p7, p8, "sd");
00022 Serial pc(USBTX, USBRX);
00023 Serial xbee(p28,p27);
00024 Serial gps(p13, p14);
00025 MS5607I2C ms5607(SDA, SCL, true);
00026 ADXL345_I2C accelerometer(SDA, SCL);
00027 HMC5883L hmc5883l(SDA, SCL);
00028 Timer timer;
00029 Ticker decision;
00030 DigitalOut led1(LED1);
00031 DigitalOut led2(LED2);
00032 DigitalOut led3(LED3);
00033 DigitalOut led4(LED4);
00034 InterruptIn StartPort(p24);
00035 PwmOut servoLeft(Ls);
00036 PwmOut servoRight(Rs);
00037 
00038 FILE *dt;
00039 FILE *lo;
00040 
00041 bool flight_pin = false;
00042 bool gp = false;
00043 int i,rlock,mode,num;
00044 char gps_data[256];
00045 char ns,ew;
00046 float w_time,hokui,tokei;
00047 float g_hokui,g_tokei;
00048 // float d_hokui,m_hokui,d_tokei,m_tokei;
00049 unsigned char c;
00050 double dev = 0;
00051 double gpsdata[4];                 //gps data
00052 
00053 
00054 void ini();
00055 void FP();
00056 void getGPS();
00057 void getdata(double *aldata, double acdata[], double mgdata[]);
00058 void servo(float x);
00059 void correction(double ac[], double mg[], double delta[]);
00060 void dev_calc(double exam[], double *s, double *t);
00061 void calc_dir_lati_longi(double newl[], double *result);
00062 
00063 
00064 int main(){
00065     pc.baud(9600);                     //sekect baudrate
00066     gps.baud(9600);
00067     xbee.baud(9600);
00068     double aldata;
00069     double acdata[3];
00070     double mgdata[3];
00071     double delta[2];
00072     double result;
00073     double chigh = 2000;
00074     double dhigh = 0;
00075     int k;
00076     mkdir("/sd/log", 0777);
00077     mkdir("/sd/sensordata", 0777);
00078     lo = fopen("/sd/log/log.txt", "a");
00079     if(lo == NULL) {
00080         error("Could not open file for write\n");
00081     }
00082     dt = fopen("/sd/sensordata/data.csv", "a");
00083     if(dt == NULL) {
00084         error("Could not open file for write\n");
00085     }
00086     fprintf(dt,"UTC,lon,lat,satnum,altitude,acce_x,acce_y,acce_z,mag_x,mag_y,mag_z\n");
00087     fclose(dt);
00088     pc.printf("Cansat activate.....\r\n");
00089     xbee.printf("Cansat activate.....\r\n");
00090     fprintf(lo,"Cansat activate.....\r\n");
00091     pc.printf("make directory and start initialize\r\n");
00092     xbee.printf("make directory and start initialize\r\n");
00093     fprintf(lo,"make directory and start initialize\r\n");
00094     ini();                         //initialize
00095     fclose(lo);
00096 
00097     StartPort.mode(PullUp);
00098     StartPort.fall(&FP);
00099     servo(0);
00100 
00101     while(flight_pin != true){
00102         led1 =! led1;
00103         wait(0.1);
00104     }
00105     led1 = 0;
00106     while(true){
00107         while(true){
00108             gps.attach(getGPS,Serial::RxIrq);
00109             if(gp == true)break;
00110         }
00111         gp=false;
00112         getdata(&aldata,acdata,mgdata);
00113         dhigh = chigh - aldata;
00114         chigh = aldata;
00115         dt = fopen("/sd/sensordata/data.csv", "a");
00116         if(dt == NULL) {
00117             error("Could not open file for write\n");
00118         }
00119         fprintf(dt,"%lf,%lf,%lf,%2.0lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",gpsdata[0],gpsdata[1],gpsdata[2],gpsdata[3],aldata,acdata[0],acdata[1],acdata[2],mgdata[0],mgdata[1],mgdata[2]);
00120         fclose(dt);
00121         correction(acdata,mgdata,delta);
00122         calc_dir_lati_longi(gpsdata,&result);
00123         dev_calc(delta,&result,&dev);
00124         lo = fopen("/sd/log/log.txt", "a");
00125         if(lo == NULL) {
00126             error("Could not open file for write\n");
00127         }
00128         if (-180 < dev && dev <= -150) {
00129             servo(0);
00130             fprintf(lo,"Left:180\r\n");
00131             pc.printf("Left:180\r\n");         
00132         }
00133         else if (-150 < dev && dev <= -120) {
00134             servo(30);
00135             fprintf(lo,"Left:150\r\n");
00136             pc.printf("Left:150\r\n");    
00137         }
00138         else if (-120 < dev && dev <= -90) {
00139             servo(60);
00140             fprintf(lo,"Left:120\r\n");
00141             pc.printf("Left:120\r\n");
00142         }
00143         else if (-90 < dev && dev <= -60) {
00144             servo(90);
00145             fprintf(lo,"Left:90\r\n");
00146             pc.printf("Left:90\r\n");
00147             }
00148         else if (-60 < dev && dev <= -30) {
00149             servo(120);
00150             fprintf(lo,"Left:60\r\n");
00151             pc.printf("Left:60\r\n");
00152         }
00153         else if(-30 < dev && dev < 0) {
00154             servo(150);
00155             fprintf(lo,"Left:30\r\n");
00156             pc.printf("Left:30\r\n");
00157         }
00158         else if (dev == 0) {
00159             servo(0);
00160             fprintf(lo,"Straight\r\n");
00161             pc.printf("Straight\r\n");
00162         }
00163         else if (0 < dev && dev <= 30) {
00164             servo(30);
00165             fprintf(lo,"Right:30\r\n");
00166             pc.printf("Right:30\r\n");
00167         }
00168         else if (30 < dev && dev <= 60) {
00169             servo(60);
00170             fprintf(lo,"Right:60\r\n");
00171             pc.printf("Right:60\r\n");
00172         }
00173         else if (60 < dev && dev <= 90) {
00174             servo(90);
00175             fprintf(lo,"Right:90\r\n");
00176             pc.printf("Right:90\r\n");
00177         }
00178         else if (90 < dev && dev <= 120) {
00179             servo(120);
00180             fprintf(lo,"Right:120\r\n");
00181             pc.printf("Right:120\r\n");
00182         }
00183         else if (120 < dev && dev <= 150) {
00184             servo(150);
00185             fprintf(lo,"Right:150\r\n");
00186             pc.printf("Right:150\r\n");
00187         }
00188         else if (150 < dev && dev <= 180) {
00189             servo(180);
00190             fprintf(lo,"Right:180\r\n");
00191             pc.printf("Right:180\r\n");
00192         }
00193         fclose(lo);
00194         /*
00195         if (-0.1 < dhigh < 0.1) {
00196             k++;
00197                 if (k >= 20) {
00198                     break;
00199                 }   
00200         }
00201         else {
00202             k = 0;
00203         }
00204         */
00205     }
00206     pc.printf("Cansat touchdown.....\r\n");
00207     xbee.printf("Cansat touchdown.....\r\n");
00208     fprintf(lo,"Cansat touchdown.....\r\n");
00209     fclose(lo);
00210     while(true){
00211         led4=!led4;
00212     }
00213     return 0;
00214 }
00215 
00216 void ini(){
00217     //accelermeter initialization
00218     accelerometer.setPowerControl(0x00);                //Go into standby mode to configure the device.
00219     accelerometer.setDataFormatControl(0x0B);           //Full resolution, +/-16g, 4mg/LSB.
00220     accelerometer.setDataRate(ADXL345_3200HZ);          //3.2kHz data rate.
00221     accelerometer.setPowerControl(0x08);                //Measurement mode.   
00222     pc.printf("All sensor is initialized\r\n");
00223     xbee.printf("All sensor is initialized\r\n");
00224     fprintf(lo,"All sensor is initialized\r\n");
00225 }
00226 
00227 void FP(){
00228     flight_pin = true;
00229 }
00230 
00231 void getGPS() {
00232     
00233   c = gps.getc();
00234   if( c=='$' || i == 256){
00235     mode = 0;
00236     i = 0;
00237     for(int j=0; j<256; j++){
00238         gps_data[j]=NULL;
00239     }
00240   }
00241   if(mode==0){
00242     if((gps_data[i]=c) != '\r'){
00243       i++;
00244     }else{
00245       gps_data[i]='\0';
00246       
00247       if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock,&num) >= 1){
00248           led1=!led1;
00249         if(rlock==1){
00250           //pc.printf("Status:Lock(%d)\n\r",rlock);
00251           //logitude
00252         //   d_tokei= int(tokei/100);
00253         //   m_tokei= (tokei-d_tokei*100)/60;
00254         //   g_tokei= d_tokei+m_tokei;
00255           //Latitude
00256         //   d_hokui=int(hokui/100);
00257         //   m_hokui=(hokui-d_hokui*100)/60;
00258         //   g_hokui=d_hokui+m_hokui;
00259           pc.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num);
00260           xbee.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num);
00261           gpsdata[0] = (double)w_time;
00262           gpsdata[2] = (double)tokei;
00263           gpsdata[1] = (double)hokui;
00264           gpsdata[3] = (double)num;
00265           gp = true;
00266         }
00267         else{
00268         //   pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
00269           pc.printf("%s\r\n",gps_data);
00270           //xbee.printf("\n\rStatus:unLock(%d)\n\r",rlock);
00271           xbee.printf("%s\r\n",gps_data);
00272         //   gp = true;
00273         }
00274         sprintf(gps_data, "");
00275       }//if
00276     }
00277   }
00278 }
00279 
00280 void getdata(double *aldata, double acdata[], double mgdata[]){
00281     led2=!led2;
00282     int readings[3];
00283     *aldata = ms5607.getAltitude();
00284     accelerometer.getOutput(readings);
00285     acdata[0] = (int16_t)readings[1]*0.039;
00286     acdata[1] = (int16_t)readings[0]*0.039*(-1);
00287     acdata[2] = (int16_t)readings[2]*0.039;
00288     mgdata[0] = (double)hmc5883l.getMy();
00289     mgdata[1] = (double)hmc5883l.getMx()*(-1);
00290     mgdata[2] = (double)hmc5883l.getMz();
00291 }
00292 void correction(double ac[], double mg[], double delta[2]) {
00293     double Hx, Hy, r, p;
00294     if ((-1 < ac[0])&&(ac[0] < 1) && (-1 < (ac[1] / sqrt(1 - ac[0] * ac[0])))&&((ac[1] / sqrt(1 - ac[0] * ac[0])) < 1)) {
00295         led3=!led3;
00296         r = asin(ac[0]);
00297         p = asin(ac[1] / sqrt(1 - ac[0] * ac[0]));
00298         Hx = cos(r) * mg[0] + sin(p) * sin(r) * mg[1] - cos(p) * sin(r) * mg[2];
00299         Hy = cos(p) * mg[1] + sin(p) * mg[2];
00300         delta[0]=Hx;
00301         delta[1]=Hy;
00302         /*
00303         *delta = atan2(Hy, Hx);
00304         if (*delta < 0)
00305             *delta += 2 * PI;
00306         if (*delta > 2 * PI)
00307             *delta -= 2 * PI;
00308         *delta = *delta * 180 / PI;
00309         */
00310     }
00311 }
00312 
00313 void servo(float x) {
00314     
00315     float y;
00316     y = 0.000284*(x / 30) + 0.0007; //left
00317 
00318     if (dev < 0) {
00319         servoLeft.pulsewidth(y);
00320     }
00321     else if(dev == 0){
00322         servoLeft.pulsewidth(y+0.000284*(180 / 30));
00323         servoRight.pulsewidth(y);
00324     }
00325     else {
00326         servoRight.pulsewidth(y);
00327     }
00328 
00329 }
00330 void dev_calc(double exam[], double *s, double *t) {
00331     double nowdir;
00332     nowdir = atan(exam[1] / exam[0]);
00333     if(nowdir < 0)nowdir += 2*PI;
00334     if(nowdir > 2*PI)nowdir -= 2*PI;
00335     nowdir = nowdir * 180 / PI;
00336     if (*s > nowdir) {
00337         if ((180 - (*s - nowdir)) < 0) {
00338             *t = *s - nowdir - 360;   //dir_lati_longi:*s  dev;*t
00339         }
00340         else {
00341             *t = *s - nowdir;
00342         }
00343     }
00344     else {
00345         if ((180 - (nowdir - *s)) < 0) {
00346             *t = 360 - nowdir + *s;
00347         }
00348         else {
00349             *t = *s - nowdir;
00350         }
00351     }
00352     pc.printf("%lf\r\n",*t);
00353 }
00354 void calc_dir_lati_longi(double newl[], double *result) {
00355     double diflongi, diflati; //difference
00356     diflati = (goallat) - newl[1]; //difference
00357     diflongi = (goallon) - newl[2];
00358     if(diflongi == 0 && diflati >= 0)*result = 0;
00359     else if(diflongi == 0 && diflati < 0)*result = 180;
00360     else if(diflongi > 0 && diflati == 0)*result = 90;
00361     else if(diflongi < 0 && diflati == 0)*result = 270;
00362     else if (diflongi >= 0 && diflati >= 0) { //1
00363         *result = atan(diflongi / diflati) * 180 / PI;
00364     }
00365     else if (diflongi >= 0 && diflati < 0) { //4
00366         *result = atan(diflongi / diflati) * 180 / PI + 180;
00367     }
00368     else if (diflongi < 0 && diflati >= 0) { //2
00369         *result = atan(diflongi / diflati) * 180 / PI + 360;
00370     }
00371     else if (diflongi < 0 && diflati < 0) { //3
00372         *result = atan(diflongi / diflati) * 180 / PI + 180;
00373     }
00374 }