temp
Dependencies: mbed SDFileSystem MS5607 ADXL345_I2C FATFileSystem
Diff: main.cpp
- Revision:
- 0:c88c3b616c00
diff -r 000000000000 -r c88c3b616c00 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Mar 16 23:37:42 2020 +0900 @@ -0,0 +1,374 @@ +#include<mbed.h> +#include"MS5607I2C.h" +#include"ADXL345_I2C.h" +#include "SDFileSystem.h" +#include "HMC5883L.h" +#include<stdio.h> +//#include<math.h> + +#define SDA p9 +#define SCL p10 +#define Rs p21 //right servo motor +#define Ls p22 //left servo motor +#define PI 3.14159265 +#define N_SAMPLE 256 +#define WAIT_TIME 50 +#define N_DIM 4 +#define EPS 1e-8 +#define goallat 4008.544783 +#define goallon 13959.2389 + +SDFileSystem sd(p5, p6, p7, p8, "sd"); +Serial pc(USBTX, USBRX); +Serial xbee(p28,p27); +Serial gps(p13, p14); +MS5607I2C ms5607(SDA, SCL, true); +ADXL345_I2C accelerometer(SDA, SCL); +HMC5883L hmc5883l(SDA, SCL); +Timer timer; +Ticker decision; +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); +InterruptIn StartPort(p24); +PwmOut servoLeft(Ls); +PwmOut servoRight(Rs); + +FILE *dt; +FILE *lo; + +bool flight_pin = false; +bool gp = false; +int i,rlock,mode,num; +char gps_data[256]; +char ns,ew; +float w_time,hokui,tokei; +float g_hokui,g_tokei; +// float d_hokui,m_hokui,d_tokei,m_tokei; +unsigned char c; +double dev = 0; +double gpsdata[4]; //gps data + + +void ini(); +void FP(); +void getGPS(); +void getdata(double *aldata, double acdata[], double mgdata[]); +void servo(float x); +void correction(double ac[], double mg[], double delta[]); +void dev_calc(double exam[], double *s, double *t); +void calc_dir_lati_longi(double newl[], double *result); + + +int main(){ + pc.baud(9600); //sekect baudrate + gps.baud(9600); + xbee.baud(9600); + double aldata; + double acdata[3]; + double mgdata[3]; + double delta[2]; + double result; + double chigh = 2000; + double dhigh = 0; + int k; + mkdir("/sd/log", 0777); + mkdir("/sd/sensordata", 0777); + lo = fopen("/sd/log/log.txt", "a"); + if(lo == NULL) { + error("Could not open file for write\n"); + } + dt = fopen("/sd/sensordata/data.csv", "a"); + if(dt == NULL) { + error("Could not open file for write\n"); + } + fprintf(dt,"UTC,lon,lat,satnum,altitude,acce_x,acce_y,acce_z,mag_x,mag_y,mag_z\n"); + fclose(dt); + pc.printf("Cansat activate.....\r\n"); + xbee.printf("Cansat activate.....\r\n"); + fprintf(lo,"Cansat activate.....\r\n"); + pc.printf("make directory and start initialize\r\n"); + xbee.printf("make directory and start initialize\r\n"); + fprintf(lo,"make directory and start initialize\r\n"); + ini(); //initialize + fclose(lo); + + StartPort.mode(PullUp); + StartPort.fall(&FP); + servo(0); + + while(flight_pin != true){ + led1 =! led1; + wait(0.1); + } + led1 = 0; + while(true){ + while(true){ + gps.attach(getGPS,Serial::RxIrq); + if(gp == true)break; + } + gp=false; + getdata(&aldata,acdata,mgdata); + dhigh = chigh - aldata; + chigh = aldata; + dt = fopen("/sd/sensordata/data.csv", "a"); + if(dt == NULL) { + error("Could not open file for write\n"); + } + 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]); + fclose(dt); + correction(acdata,mgdata,delta); + calc_dir_lati_longi(gpsdata,&result); + dev_calc(delta,&result,&dev); + lo = fopen("/sd/log/log.txt", "a"); + if(lo == NULL) { + error("Could not open file for write\n"); + } + if (-180 < dev && dev <= -150) { + servo(0); + fprintf(lo,"Left:180\r\n"); + pc.printf("Left:180\r\n"); + } + else if (-150 < dev && dev <= -120) { + servo(30); + fprintf(lo,"Left:150\r\n"); + pc.printf("Left:150\r\n"); + } + else if (-120 < dev && dev <= -90) { + servo(60); + fprintf(lo,"Left:120\r\n"); + pc.printf("Left:120\r\n"); + } + else if (-90 < dev && dev <= -60) { + servo(90); + fprintf(lo,"Left:90\r\n"); + pc.printf("Left:90\r\n"); + } + else if (-60 < dev && dev <= -30) { + servo(120); + fprintf(lo,"Left:60\r\n"); + pc.printf("Left:60\r\n"); + } + else if(-30 < dev && dev < 0) { + servo(150); + fprintf(lo,"Left:30\r\n"); + pc.printf("Left:30\r\n"); + } + else if (dev == 0) { + servo(0); + fprintf(lo,"Straight\r\n"); + pc.printf("Straight\r\n"); + } + else if (0 < dev && dev <= 30) { + servo(30); + fprintf(lo,"Right:30\r\n"); + pc.printf("Right:30\r\n"); + } + else if (30 < dev && dev <= 60) { + servo(60); + fprintf(lo,"Right:60\r\n"); + pc.printf("Right:60\r\n"); + } + else if (60 < dev && dev <= 90) { + servo(90); + fprintf(lo,"Right:90\r\n"); + pc.printf("Right:90\r\n"); + } + else if (90 < dev && dev <= 120) { + servo(120); + fprintf(lo,"Right:120\r\n"); + pc.printf("Right:120\r\n"); + } + else if (120 < dev && dev <= 150) { + servo(150); + fprintf(lo,"Right:150\r\n"); + pc.printf("Right:150\r\n"); + } + else if (150 < dev && dev <= 180) { + servo(180); + fprintf(lo,"Right:180\r\n"); + pc.printf("Right:180\r\n"); + } + fclose(lo); + /* + if (-0.1 < dhigh < 0.1) { + k++; + if (k >= 20) { + break; + } + } + else { + k = 0; + } + */ + } + pc.printf("Cansat touchdown.....\r\n"); + xbee.printf("Cansat touchdown.....\r\n"); + fprintf(lo,"Cansat touchdown.....\r\n"); + fclose(lo); + while(true){ + led4=!led4; + } + return 0; +} + +void ini(){ + //accelermeter initialization + accelerometer.setPowerControl(0x00); //Go into standby mode to configure the device. + accelerometer.setDataFormatControl(0x0B); //Full resolution, +/-16g, 4mg/LSB. + accelerometer.setDataRate(ADXL345_3200HZ); //3.2kHz data rate. + accelerometer.setPowerControl(0x08); //Measurement mode. + pc.printf("All sensor is initialized\r\n"); + xbee.printf("All sensor is initialized\r\n"); + fprintf(lo,"All sensor is initialized\r\n"); +} + +void FP(){ + flight_pin = true; +} + +void getGPS() { + + c = gps.getc(); + if( c=='$' || i == 256){ + mode = 0; + i = 0; + for(int j=0; j<256; j++){ + gps_data[j]=NULL; + } + } + if(mode==0){ + if((gps_data[i]=c) != '\r'){ + i++; + }else{ + gps_data[i]='\0'; + + if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock,&num) >= 1){ + led1=!led1; + if(rlock==1){ + //pc.printf("Status:Lock(%d)\n\r",rlock); + //logitude + // d_tokei= int(tokei/100); + // m_tokei= (tokei-d_tokei*100)/60; + // g_tokei= d_tokei+m_tokei; + //Latitude + // d_hokui=int(hokui/100); + // m_hokui=(hokui-d_hokui*100)/60; + // g_hokui=d_hokui+m_hokui; + pc.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num); + xbee.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num); + gpsdata[0] = (double)w_time; + gpsdata[2] = (double)tokei; + gpsdata[1] = (double)hokui; + gpsdata[3] = (double)num; + gp = true; + } + else{ + // pc.printf("\n\rStatus:unLock(%d)\n\r",rlock); + pc.printf("%s\r\n",gps_data); + //xbee.printf("\n\rStatus:unLock(%d)\n\r",rlock); + xbee.printf("%s\r\n",gps_data); + // gp = true; + } + sprintf(gps_data, ""); + }//if + } + } +} + +void getdata(double *aldata, double acdata[], double mgdata[]){ + led2=!led2; + int readings[3]; + *aldata = ms5607.getAltitude(); + accelerometer.getOutput(readings); + acdata[0] = (int16_t)readings[1]*0.039; + acdata[1] = (int16_t)readings[0]*0.039*(-1); + acdata[2] = (int16_t)readings[2]*0.039; + mgdata[0] = (double)hmc5883l.getMy(); + mgdata[1] = (double)hmc5883l.getMx()*(-1); + mgdata[2] = (double)hmc5883l.getMz(); +} +void correction(double ac[], double mg[], double delta[2]) { + double Hx, Hy, r, p; + 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)) { + led3=!led3; + r = asin(ac[0]); + p = asin(ac[1] / sqrt(1 - ac[0] * ac[0])); + Hx = cos(r) * mg[0] + sin(p) * sin(r) * mg[1] - cos(p) * sin(r) * mg[2]; + Hy = cos(p) * mg[1] + sin(p) * mg[2]; + delta[0]=Hx; + delta[1]=Hy; + /* + *delta = atan2(Hy, Hx); + if (*delta < 0) + *delta += 2 * PI; + if (*delta > 2 * PI) + *delta -= 2 * PI; + *delta = *delta * 180 / PI; + */ + } +} + +void servo(float x) { + + float y; + y = 0.000284*(x / 30) + 0.0007; //left + + if (dev < 0) { + servoLeft.pulsewidth(y); + } + else if(dev == 0){ + servoLeft.pulsewidth(y+0.000284*(180 / 30)); + servoRight.pulsewidth(y); + } + else { + servoRight.pulsewidth(y); + } + +} +void dev_calc(double exam[], double *s, double *t) { + double nowdir; + nowdir = atan(exam[1] / exam[0]); + if(nowdir < 0)nowdir += 2*PI; + if(nowdir > 2*PI)nowdir -= 2*PI; + nowdir = nowdir * 180 / PI; + if (*s > nowdir) { + if ((180 - (*s - nowdir)) < 0) { + *t = *s - nowdir - 360; //dir_lati_longi:*s dev;*t + } + else { + *t = *s - nowdir; + } + } + else { + if ((180 - (nowdir - *s)) < 0) { + *t = 360 - nowdir + *s; + } + else { + *t = *s - nowdir; + } + } + pc.printf("%lf\r\n",*t); +} +void calc_dir_lati_longi(double newl[], double *result) { + double diflongi, diflati; //difference + diflati = (goallat) - newl[1]; //difference + diflongi = (goallon) - newl[2]; + if(diflongi == 0 && diflati >= 0)*result = 0; + else if(diflongi == 0 && diflati < 0)*result = 180; + else if(diflongi > 0 && diflati == 0)*result = 90; + else if(diflongi < 0 && diflati == 0)*result = 270; + else if (diflongi >= 0 && diflati >= 0) { //1 + *result = atan(diflongi / diflati) * 180 / PI; + } + else if (diflongi >= 0 && diflati < 0) { //4 + *result = atan(diflongi / diflati) * 180 / PI + 180; + } + else if (diflongi < 0 && diflati >= 0) { //2 + *result = atan(diflongi / diflati) * 180 / PI + 360; + } + else if (diflongi < 0 && diflati < 0) { //3 + *result = atan(diflongi / diflati) * 180 / PI + 180; + } +}