2017能代宇宙イベント開放用プログラム(kobayashi)
Dependencies: mbed MS5607 MPU6050
Diff: main.cpp
- Revision:
- 3:507d05fb5cd8
- Parent:
- 2:3a7c38279b0b
- Child:
- 4:a44c7a8adaeb
--- a/main.cpp Sat Aug 05 13:44:05 2017 +0000 +++ b/main.cpp Sun Aug 06 07:00:12 2017 +0000 @@ -1,42 +1,116 @@ #include "mbed.h" #include "MS5607I2C.h" +#include "MPU6050.h" +#include <math.h> -#define RISE 0 -#define DROP 1 +#define START_kasokudo 0 +#define START_koudo 0 +#define P_openkoudo 0.5 +#define SETUP 0 +#define LAUNCH 1 +#define RISE 2 +#define DROP1 3 +#define DROP2 4 +#define LAND 5 MS5607I2C ms(p9,p10,false); -DigitalOut myled(LED1); -pwmout servo1(p23); -int Phase=RISE; +MPU6050 mpu(p9,p10); +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3);//後で削除// +DigitalOut myled4(LED4); +PwmOut servo1(p23); +PwmOut servo2(p24); +PwmOut servo3(p25); +int Phase=SETUP; Ticker tic; float max=0; -int count=0; +int count_LAUNCH=0; +int count_RISE=0; +int count_DROP1=0; +Timer Time; +float LAUNCHtime,RISEtime,DROP1time; -void judge(){ +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; + case RISE: - float alt = ms.getAltitude(); - if(max <= alt){ - max = alt; - }else if(max > alt){ - max = alt; - count++; + myled2=0; + float alt_RISE = ms.getAltitude(); + if(max < alt_RISE){ + max = alt_RISE; + }else if(max+10 >= alt_RISE){ + max = alt_RISE; + count_RISE++; + RISEtime = Time.read(); + } + if(Time.read() >= RISEtime + 0.5){ + count_RISE = 0; + } + if(count_RISE==5);{ + servo1.pulsewidth(0.0015); + servo2.pulsewidth(0.0015); + wait(2.0); + myled2 = 1; Phase=DROP1; } break; case DROP1: - if(count=5){ - servo1.pulsewidth(90); - wait(1.0); + 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; } - float alt = ms.getAltitude(); break; + + case DROP2: + if(ms.getAltitude()== START_koudo){ + //ここでサーボ電源切る卍卍// + Phase=LAND; + } + break; + + + case LAND: + break; + } } int main(){ - timer start(); - tic.attach(&judge,0.1); + Timer start(); + Time.start(); + tic.attach(&_judge,0.1); while(1); return 0; } \ No newline at end of file