2017能代宇宙イベント開放用プログラム(kobayashi)
Dependencies: mbed MS5607 MPU6050
main.cpp
- Committer:
- nene_kobayashi
- Date:
- 2017-08-06
- Revision:
- 4:a44c7a8adaeb
- Parent:
- 3:507d05fb5cd8
- Child:
- 5:e5c45611bf43
File content as of revision 4:a44c7a8adaeb:
#include "mbed.h" #include "MS5607I2C.h" #include "MPU6050.h" #include <math.h> #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 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; void _judge(); float _mannaka int main(){ Timer start(); Time.start(); tic.attach(&_judge,0.1); while(1); return 0; } 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: 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; 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; case DROP2: if(ms.getAltitude()== START_koudo){ //ここでサーボ電源切る卍卍// Phase=LAND; } break; case LAND: break; } } float _mannaka{