Diff: main.cpp

Revision:

1:68309ba0103d

Parent:

0:a4734a842395

--- a/main.cpp	Sun Jul 23 03:34:48 2017 +0000
+++ b/main.cpp	Fri Aug 11 13:57:14 2017 +0000
@@ -1,12 +1,79 @@
 #include "mbed.h"
+#include "math.h"
+#include "MPU6050.h"
+#include "MS5607I2C.h"
+
+#define STANBY
+#define LAUNCH
+#define RISE
+#define DROP
+#define  ACC
+#define SERCH_TIME
+#define
+#define
+#define
+#define
 
-DigitalOut myled(LED1);
+
+MPU6050 mpu(p9, p10);
+MS5607I2C ms(p9, p10, false);
+PwmOut servo1(p26);
+PwmOut servo2(p25);
+PwmOut servo3(p24);
+DegitalOut myled1(LED1);
+DegitalOut myled2(LED2);
+DegitalOut myled3(LED3);
+Timer timer;
+Ticker ticker;
+
+
+void _judge();
+float _median(float[5]);
+
 
 int main() {
-    while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
-    }
+    timer.start()
 }
+
+void _judge(){
+    switch(FHASE){
+        case STANBY:
+                    break;
+        case LAUNCH:
+                    myled1 = 0;
+                    float acc[3], acc_3;
+                    int count = 0;
+                    
+                    getAccelero(acc);
+                    acc_3 = pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2);
+                    acc_3 = sqrt(acc_3);
+                    
+                    if(acc >= ACC){
+                        if(count == 0) SERCH_TIME = timer.read();
+                        count++;
+                    }
+                    
+                    if(timer.read() > time + SERCH_TIME){
+                        count = 0;
+                    }
+                    
+                    if(count == COUNT){
+                        myled = 1;
+                        PHASE = RISE;
+                    }
+                    
+                    break;
+        case RISE:
+                    myled2 = 0;
+                    
+                    break;
+        case DROP:
+                    
+                    break;
+        
+        }
+}
+
+float _median(){
+    
+}