克輝 水上
きりたんぽの開放プログラム
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of
SDFileSystem_HelloWorld
by 
mbed official
Revision 2:4cdbd4b3722d, committed 2018-07-31
- Comitter:
- mizukamikatsuki
- Date:
- Tue Jul 31 11:43:36 2018 +0000
- Parent:
- 1:e4d7342be507
- Commit message:
- ?????????????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMP180.lib Tue Jul 31 11:43:36 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/spiridion/code/BMP180/#072073c79cfd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Tue Jul 31 11:43:36 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
--- a/SDFileSystem.lib Tue May 16 05:18:55 2017 +0000 +++ b/SDFileSystem.lib Tue Jul 31 11:43:36 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec +http://mbed.org/users/neilt6/code/SDFileSystem/#e4d2567200db
--- a/main.cpp Tue May 16 05:18:55 2017 +0000
+++ b/main.cpp Tue Jul 31 11:43:36 2018 +0000
@@ -1,19 +1,338 @@
#include "mbed.h"
+#include "math.h"
+#include "MPU6050.h"
+#include "BMP180.h"
#include "SDFileSystem.h"
-
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
-
-int main() {
- printf("Hello World!\n");
-
- mkdir("/sd/mydir", 0777);
+
+#define p0 1013.25f
+#define JUDGE_ACC 1.0
+#define JUDGE_ALT 1.0
+#define RATE_GET 20
+#define RATE_OPEN 20
+#define NUM_DATA 20
+#define NUMBER 5
+#define TIME_OPEN 14
+#define TIME_DROP 104
+
+enum PHASE{SETUP,LAUNCH,RISE,DROP,LAND} Phase;
+
+Serial twe(p13,p14);
+Serial pc(USBTX,USBRX);
+MPU6050 mpu(p9,p10);
+BMP180 bmp(p9, p10);
+SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+Serial gps(p28,p27);
+PwmOut servo1(p21);
+PwmOut servo2(p25);
+DigitalOut fte(p5);
+DigitalOut buzz(p6);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+
+
+Timer timer_open;
+Timer timer_log;
+Ticker tic_open;
+Ticker tic_log;
+Ticker tic_heat;
+Ticker tic_buzz;
+
+
+FILE *bp;
+FILE *lfp;
+LocalFileSystem local("local");
+
+void _open();
+void _getData();
+int _input(char cha);
+void _heater();
+void _buzzer();
+char comh;
+char comb;
+float _median(float data[], int num);
+float getAlt(float press, float temp);
+float _DMS2DEG(float raw_data);
+
+float Alt_gnd;
+float med[NUMBER];
+float alt[NUMBER];
+float med_alt;
+float max_alt=-1000;
+float mae_alt=5000;
+float med_g;
+float pressure,temperature,altitude;
+float Time;
+float Data[2][NUM_DATA][7]={0};
+char gps_data[256];
+int cnt_gps;
+int Cnt;
+int cnt_alt=0;
+int cnt_d=0;
+int Cnt_Data;
+int Cnt_GPS=0;
+bool row;
+
+int main(){
+ twe.baud(115200);
+ mpu.setAcceleroRange(3);
+ bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
+ mkdir("/sd/mydir",0777);
+ bp=fopen("/sd/mydir/data.bin","ab");
+ fclose(bp);
+ lfp=fopen("/local/data.txt","a");
+ fclose(lfp);
+ servo1.period_ms(20);
+ servo2.period_ms(20);
+ led1=0;
+ led2=0;
+ led3=0;
+twe.printf("HELLO!\r\n");
+ led1=1;
+ led2=1;
+ led3=1;
+ led4=1;
+ fte=0;
+ buzz=0;
+ pc.printf("hello\r\n");
- FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
- if(fp == NULL) {
- error("Could not open file for write\n");
+ while(1){
+ char cha = twe.getc();
+ if(_input(cha)==-1){
+ timer_log.start();
+ tic_log.attach(&_getData, 1.0/RATE_GET);
+ break;
+ }
+ }
+ tic_heat.attach(&_heater,0.1);
+ led1=0;
+ led2=0;
+ led3=0;
+ led4=0;
+ Phase = SETUP;
+ tic_open.attach(&_open, 1.0/RATE_OPEN);
+
+ while(1){
+
+
+ if(gps.readable()){
+ if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
+ cnt_gps = 0;
+ memset(gps_data,'\0',256);
+ }else if(gps_data[cnt_gps] == '\r'){
+ float world_time, lon_east, lat_north;
+ int rlock, sat_num;
+ char lat,lon;
+ if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
+ if(rlock==1){
+ lat_north = _DMS2DEG(lat_north);
+ lon_east = _DMS2DEG(lon_east);
+ twe.printf("%s\r\n",gps_data);
+ twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+ }else{
+ twe.printf("%s\r\n",gps_data);
+ }
+ }
+ }else{
+ cnt_gps++;
+ }
+ }
+
+
+ if(timer_log.read()>=30.0*60.0) timer_log.reset();
}
- fprintf(fp, "Hello fun SD Card World!");
- fclose(fp);
+}
+
+
+
+void _open(){
+ switch(Phase){
+ case SETUP:
+ for(Cnt=0;Cnt<10;Cnt++){
+ bmp.ReadData(&temperature,&pressure);
+ altitude = getAlt(pressure,temperature);
+ alt[Cnt]=altitude;
+ }
+ Alt_gnd = _median(alt,10);
+ lfp=fopen("/local/data.txt", "a");
+ fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
+ fclose(lfp);
+ Phase = LAUNCH;
+ twe.printf("Phase=LAUNCH\r\n");
+ pc.printf("Phase=LAUNCH\r\n");
+ break;
+
+ case LAUNCH:
+
+ float a[3],g;
+ int i=0;
+ for(i=0;i<NUMBER;i++){
+ mpu.getAccelero(a);
+ g=sqrt(pow(a[0],2)+(a[1],2)+(a[2],2))/9.81;
+ med[i]=g;
+ }
+ med_g=_median(med,NUMBER);
+ twe.printf("%f\n\r",med_g);
+ if(med_g>JUDGE_ACC){
+ tic_heat.detach();
+ timer_open.start();
+ pc.printf("発射\r\n");
+ twe.printf("発射\r\n");
+ led1=0;
+ led4=0;
+ Phase=RISE;
+ twe.printf("PHASE=RISE\r\n");
+ }
+ led1=!led1;
+ break;
+
+ case RISE:
+ int j=0;
+ for(j=0;j<NUMBER;j++){
+ bmp.ReadData(&temperature,&pressure);
+ altitude = getAlt(pressure,temperature);
+ alt[j]=altitude;
+ }
+ med_alt=_median(alt,NUMBER);
+ twe.printf("%f, %f\r\n",max_alt,med_alt);
+ if(med_alt>max_alt){
+ max_alt=med_alt;
+ cnt_alt=0;
+ }else if(max_alt-med_alt>JUDGE_ALT){
+ cnt_alt++;
+ }
+ twe.printf("%d\n\r",cnt_alt);
+ if(cnt_alt>=3 || timer_open.read()>=TIME_OPEN){
+ pc.printf("頂点\r\n");
+ twe.printf("頂点\r\n");
+ lfp = fopen("/local/data.txt", "a");
+ fprintf(lfp,"最高高度：%f\r\n",max_alt);
+ fclose(lfp);
+ servo1.pulsewidth(0.0015);
+ servo2.pulsewidth(0.0006);
+ led2=0;
+ Phase=DROP;
+ twe.printf("PHASE=DROP");
+ }
+ led2=!led2;
+ break;
+
+ case DROP:
+ int k=0;
+ for(k=0;k<NUMBER;k++){
+ bmp.ReadData(&temperature,&pressure);
+ altitude = getAlt(pressure,temperature);
+ alt[k]=altitude;
+ }
+ med_alt=_median(alt,NUMBER);
+ mae_alt=med_alt;
+ if(mae_alt-med_alt<=1 && mae_alt-med_alt>=-1){
+ cnt_d++;
+ }else{
+ cnt_d=0;
+ }
+ twe.printf("%d\n\r",cnt_d);
+ if(cnt_d==3 || timer_open.read()>TIME_DROP){
+ pc.printf("着水\r\n");
+ twe.printf("着水\r\n");
+ timer_open.stop();
+ tic_buzz.attach(&_buzzer,0.1);
+ Phase=LAND;
+ pc.printf("Judge DROP\r\n");
+ twe.printf("FINISH\r\n");
+ led3=0;
+ }
+ led3=!led3;
+ break;
+ }
+}
+
+void _getData(){
+ Data[row][Cnt_Data][0] = timer_log.read();
+ mpu.getAccelero(&Data[row][Cnt_Data][1]);
+ bmp.ReadData(&Data[row][Cnt_Data][4],&Data[row][Cnt_Data][5]);
+ Data[row][Cnt_Data][6] = (pow((p0/Data[row][Cnt_Data][5]), (1.0f/5.257f))-1.0f)*(Data[row][Cnt_Data][4]+273.15f)/0.0065f;
+ Cnt_Data++;
+ /*Log*/
+ if(Cnt_Data==NUM_DATA){
+ Cnt_Data = 0;
+ row =! row;
+ bp = fopen("/sd/mydir/data.bin","ab");
+ fwrite(&Data[!row][0][0],sizeof(float),7*NUM_DATA,bp);
+ fclose(bp);
+ }
+}
+
+float _DMS2DEG(float raw_data){
+ int d=(int)(raw_data/100);
+ float m=(raw_data-(float)d*100);
+ return (float)d+m/60;
+}
+
+float getAlt(float press, float temp){
+ return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
+}
+
+float _median(float data[], int num){
+ float *data_cpy, ans;
+ data_cpy = new float[num];
+ memcpy(data_cpy,data,sizeof(float)*num);
- printf("Goodbye World!\n");
+ for(int i=0; i<num; i++){
+ for(int j=0; j<num-i-1; j++){
+ if(data_cpy[j]>data_cpy[j+1]){
+ float buff = data_cpy[j+1];
+ data_cpy[j+1] = data_cpy[j];
+ data_cpy[j] = buff;
+ }
+ }
+ }
+ if(num%2!=0) ans = data_cpy[num/2];
+ else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
+ delete[] data_cpy;
+ return ans;
+}
+
+int _input(char cha){
+ if(cha=='F'){
+ twe.printf("flight mode on\r\n");
+ return -1;
+ }else if(cha=='U'){
+ servo1.pulsewidth(0.0015);
+ servo2.pulsewidth(0.0006);
+ }else if(cha=='L'){
+ servo1.pulsewidth(0.0008);
+ servo2.pulsewidth(0.0014);
+ }
+ return 0;
}
+
+void _heater(){
+ if(twe.readable()){
+ char comh=twe.getc();
+ if(comh=='H'){
+ fte=1;
+ led4=1;
+ }else if(comh=='C'){
+ fte=0;
+ led4=0;
+ }
+ }
+}
+
+void _buzzer(){
+ if(twe.readable()){
+ char comb=twe.getc();
+ if(comb=='N'){
+ buzz=1;
+ led4=1;
+ }else if(comb=='S'){
+ buzz=0;
+ led4=0;
+ }
+ }
+
+}
+