2017能代宇宙イベント開放用プログラム(kobayashi)
Dependencies: mbed MS5607 MPU6050
Revision 5:e5c45611bf43, committed 2017-08-15
- Comitter:
- nene_kobayashi
- Date:
- Tue Aug 15 09:54:44 2017 +0000
- Parent:
- 4:a44c7a8adaeb
- Commit message:
- ????????????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r a44c7a8adaeb -r e5c45611bf43 main.cpp --- a/main.cpp Sun Aug 06 09:48:17 2017 +0000 +++ b/main.cpp Tue Aug 15 09:54:44 2017 +0000 @@ -1,121 +1,113 @@ -#include "mbed.h" -#include "MS5607I2C.h" -#include "MPU6050.h" -#include <math.h> +#include <mbed.h> +#include <string.h> +#include えむぴーゆー +#include 卍卍エムエス卍卍 -#define START_kasokudo 0 -#define START_koudo 0 -#define P_openkoudo 150 -#define SETUP 0 -#define LAUNCH 1 -#define RISE 2 -#define DROP1 3 -#define DROP2 4 -#define LAND 5 +//主関数 +void _log; +void _open; -Degitalout -Serial pc(USBTX,USBRX); -MS5607I2C ms(p9,p10,false); -MPU6050 mpu(p9,p10); -DigitalOut myled1(LED1); -DigitalOut myled2(LED2); -DigitalOut myled3(LED3);//後で削除// -PwmOut servo1(p23); -PwmOut servo2(p24); -PwmOut servo3(p25); -int Phase=SETUP; -Ticker tic; -float max=0; -int count_LAUNCH=0; -int count_RISE=0; -int count_DROP1=0; -Timer Time; -float LAUNCHtime,RISEtime,DROP1time; +//雑関数 +float mannaka; +float + +#define RATE_LOG 100 +#define RATE_OPEN 20 -void _judge(); -float _mannaka +enum FHASE{SETUP=0,LAUNCH=1,RISE=3,DROP1=7,DROP2=15,LAND=16}Fhase; //二進数 + +//タイマー系 +Ticker tic_log; +Ticker tic_open; +Timer time; int main(){ - Timer start(); - Time.start(); - tic.attach(&_judge,0.1); - while(1); - return 0; + time.start(); + tic_open.attach(&_open , 1.0/RATE_LOG); + tic_open.attach(&_log , 1.0/RATE_OPEN); +} + +void _log{ + } -void _judge(){ - switch(Phase){ - case SETUP: - Phase=LAUNCH; - break; - - case LAUNCH: - myled1=0; - float acc[3]; - mpu.getAccelero(acc); - float acc_LAUNCH = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); - if(START_kasokudo < acc_LAUNCH){ - count_LAUNCH++; - LAUNCHtime = Time.read(); - } - if(Time.read() >= LAUNCHtime+0.5){ - count_LAUNCH=0; - } - if(count_LAUNCH==5){ - myled1=1; - Phase=RISE; - } - break; - +void _open{ + switch(Phase){ + case SETUP: if フライトモード解除 + Fhase = LAUNCH; + break + case LAUNCH: + 配列つくる(X,Y,Z) + タイマースタート + 加速度とる + 合成する + if 加速度(合成したもの)==一定の値 + if (時間>=TBD){ + カウント=0 + } + if (カウント==5){ + Fhase = RISE; + } + break case RISE: - myled2=0; - float alt_RISE = ms.getAltitude(); - if(max < alt_RISE+0.5){ - max = alt_RISE; - }else if(max+0.5 > alt_RISE){ - count_RISE++; - RISEtime = Time.read(); - } - if(Time.read() >= RISEtime + 1.0){ - count_RISE = 0; - } - if(count_RISE==5){ - servo1.pulsewidth(0.0015); - servo2.pulsewidth(0.0015); - myled2 = 1; - Phase=DROP1; - } - break; - + タイマー + 高度とる + _mannaka呼び出す + if (高度<=頂点){ + カウント++ + } + if (時間>=TBD){ + カウント=0 + } + if (カウント==5){ + パラシュート開放 + } + Fhase = DROP1; + break case DROP1: - myled3=0; - float alt_DROP1 = ms.getAltitude(); - if((alt_DROP1 - START_koudo) < P_openkoudo){ - count_DROP1++; - } - if(Time.read() <= DROP1time + 0.5){ - count_DROP1 = 0; - } - if(count_DROP1==5){ - servo3.pulsewidth(0.0015); - wait(2.0); - myled3 = 1; - Phase=DROP2; - } - break; - + タイマー + 高度とる + _mannaka呼び出す + if (高度<=リーふぃんぐ高度){ + カウント++ + } + if (時間>=TBD){ + カウント==0 + } + if (カウント==5){ + リーふぃんぐ開放 + } + Fhase = DROP2; + break case DROP2: - if(ms.getAltitude()== START_koudo){ - //ここでサーボ電源切る卍卍// - Phase=LAND; - } - break; - - + if 時間==TBD + さーぼの電源切る + Fhase = LAND; + break case LAND: - break; + break } } - -float _mannaka{ - \ No newline at end of file + +float mannaka(float data[],int num){ + float ans; + float *data_copy; + float data_copy = new float [num] //float型の配列を[num]分確保 + memcpy(data_copy,data,sizeof(float) *num); + for(int i=0,i<num,i++){ + for(int j=0,j<num+1,j++){ + if(copy_data[j]>copy_data[j+1]){ + int tmp = copy_data[j]; + copy_data[j] = copy_data[j+1]; + copy_data[j+1] = tmp; + } + } + } + if (num%2=0){ + ans = (data_copy[num/2]+data_copy[(num/2)+1])/2; + }else{ + ans = data_copy[num/2]; + } + delete[] data_copy; + return 0; +} \ No newline at end of file