main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "MPU6050.h"
00004 #include "MS5607I2C.h"
00005 
00006 #define STANBY
00007 #define LAUNCH
00008 #define RISE
00009 #define DROP
00010 #define  ACC
00011 #define SERCH_TIME
00012 #define
00013 #define
00014 #define
00015 #define
00016 
00017 
00018 MPU6050 mpu(p9, p10);
00019 MS5607I2C ms(p9, p10, false);
00020 PwmOut servo1(p26);
00021 PwmOut servo2(p25);
00022 PwmOut servo3(p24);
00023 DegitalOut myled1(LED1);
00024 DegitalOut myled2(LED2);
00025 DegitalOut myled3(LED3);
00026 Timer timer;
00027 Ticker ticker;
00028 
00029 
00030 void _judge();
00031 float _median(float[5]);
00032 
00033 
00034 int main() {
00035     timer.start()
00036 }
00037 
00038 void _judge(){
00039     switch(FHASE){
00040         case STANBY:
00041                     break;
00042         case LAUNCH:
00043                     myled1 = 0;
00044                     float acc[3], acc_3;
00045                     int count = 0;
00046                     
00047                     getAccelero(acc);
00048                     acc_3 = pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2);
00049                     acc_3 = sqrt(acc_3);
00050                     
00051                     if(acc >= ACC){
00052                         if(count == 0) SERCH_TIME = timer.read();
00053                         count++;
00054                     }
00055                     
00056                     if(timer.read() > time + SERCH_TIME){
00057                         count = 0;
00058                     }
00059                     
00060                     if(count == COUNT){
00061                         myled = 1;
00062                         PHASE = RISE;
00063                     }
00064                     
00065                     break;
00066         case RISE:
00067                     myled2 = 0;
00068                     
00069                     break;
00070         case DROP:
00071                     
00072                     break;
00073         
00074         }
00075 }
00076 
00077 float _median(){
00078     
00079 }