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 servo１(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