伊生希 小林
temp
Dependencies: mbed SDFileSystem MS5607 ADXL345_I2C FATFileSystem
Diff: main.cpp
- Revision:
- 0:c88c3b616c00
--- /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;
+ }
+}