nene kobayashi / Mbed 2 deprecated 201708kaiho_kobayashi

2017能代宇宙イベント開放用プログラム(kobayashi)

Dependencies:   mbed MS5607 MPU6050

main.cpp@3:507d05fb5cd8, 2017-08-06 (annotated)

Committer:
nene_kobayashi
Date:
Sun Aug 06 07:00:12 2017 +0000
Revision:
3:507d05fb5cd8
Parent:
2:3a7c38279b0b
Child:
4:a44c7a8adaeb
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nene_kobayashi 0:027a3ffb7500 1 #include "mbed.h"
nene_kobayashi 1:fa1980f9b2d0 2 #include "MS5607I2C.h"
nene_kobayashi 3:507d05fb5cd8 3 #include "MPU6050.h"
nene_kobayashi 3:507d05fb5cd8 4 #include <math.h>
nene_kobayashi 0:027a3ffb7500 5
nene_kobayashi 3:507d05fb5cd8 6 #define START_kasokudo 0
nene_kobayashi 3:507d05fb5cd8 7 #define START_koudo 0
nene_kobayashi 3:507d05fb5cd8 8 #define P_openkoudo 0.5
nene_kobayashi 3:507d05fb5cd8 9 #define SETUP 0
nene_kobayashi 3:507d05fb5cd8 10 #define LAUNCH 1
nene_kobayashi 3:507d05fb5cd8 11 #define RISE 2
nene_kobayashi 3:507d05fb5cd8 12 #define DROP1 3
nene_kobayashi 3:507d05fb5cd8 13 #define DROP2 4
nene_kobayashi 3:507d05fb5cd8 14 #define LAND 5
nene_kobayashi 2:3a7c38279b0b 15
nene_kobayashi 1:fa1980f9b2d0 16 MS5607I2C ms(p9,p10,false);
nene_kobayashi 3:507d05fb5cd8 17 MPU6050 mpu(p9,p10);
nene_kobayashi 3:507d05fb5cd8 18 DigitalOut myled1(LED1);
nene_kobayashi 3:507d05fb5cd8 19 DigitalOut myled2(LED2);
nene_kobayashi 3:507d05fb5cd8 20 DigitalOut myled3(LED3);//後で削除//
nene_kobayashi 3:507d05fb5cd8 21 DigitalOut myled4(LED4);
nene_kobayashi 3:507d05fb5cd8 22 PwmOut servo1(p23);
nene_kobayashi 3:507d05fb5cd8 23 PwmOut servo2(p24);
nene_kobayashi 3:507d05fb5cd8 24 PwmOut servo3(p25);
nene_kobayashi 3:507d05fb5cd8 25 int Phase=SETUP;
nene_kobayashi 2:3a7c38279b0b 26 Ticker tic;
nene_kobayashi 2:3a7c38279b0b 27 float max=0;
nene_kobayashi 3:507d05fb5cd8 28 int count_LAUNCH=0;
nene_kobayashi 3:507d05fb5cd8 29 int count_RISE=0;
nene_kobayashi 3:507d05fb5cd8 30 int count_DROP1=0;
nene_kobayashi 3:507d05fb5cd8 31 Timer Time;
nene_kobayashi 3:507d05fb5cd8 32 float LAUNCHtime,RISEtime,DROP1time;
nene_kobayashi 1:fa1980f9b2d0 33
nene_kobayashi 3:507d05fb5cd8 34 void _judge(){
nene_kobayashi 2:3a7c38279b0b 35 switch(Phase){
nene_kobayashi 3:507d05fb5cd8 36 case SETUP:
nene_kobayashi 3:507d05fb5cd8 37 Phase=LAUNCH;
nene_kobayashi 3:507d05fb5cd8 38 break;
nene_kobayashi 3:507d05fb5cd8 39
nene_kobayashi 3:507d05fb5cd8 40 case LAUNCH:
nene_kobayashi 3:507d05fb5cd8 41 myled1=0;
nene_kobayashi 3:507d05fb5cd8 42 float acc[3];
nene_kobayashi 3:507d05fb5cd8 43 mpu.getAccelero(acc);
nene_kobayashi 3:507d05fb5cd8 44 float acc_LAUNCH = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
nene_kobayashi 3:507d05fb5cd8 45 if(START_kasokudo < acc_LAUNCH){
nene_kobayashi 3:507d05fb5cd8 46 count_LAUNCH++;
nene_kobayashi 3:507d05fb5cd8 47 LAUNCHtime = Time.read();
nene_kobayashi 3:507d05fb5cd8 48 }
nene_kobayashi 3:507d05fb5cd8 49 if(Time.read() >= LAUNCHtime+0.5){
nene_kobayashi 3:507d05fb5cd8 50 count_LAUNCH=0;
nene_kobayashi 3:507d05fb5cd8 51 }
nene_kobayashi 3:507d05fb5cd8 52 if(count_LAUNCH==5){
nene_kobayashi 3:507d05fb5cd8 53 myled1=1;
nene_kobayashi 3:507d05fb5cd8 54 Phase=RISE;
nene_kobayashi 3:507d05fb5cd8 55 }
nene_kobayashi 3:507d05fb5cd8 56 break;
nene_kobayashi 3:507d05fb5cd8 57
nene_kobayashi 2:3a7c38279b0b 58 case RISE:
nene_kobayashi 3:507d05fb5cd8 59 myled2=0;
nene_kobayashi 3:507d05fb5cd8 60 float alt_RISE = ms.getAltitude();
nene_kobayashi 3:507d05fb5cd8 61 if(max < alt_RISE){
nene_kobayashi 3:507d05fb5cd8 62 max = alt_RISE;
nene_kobayashi 3:507d05fb5cd8 63 }else if(max+10 >= alt_RISE){
nene_kobayashi 3:507d05fb5cd8 64 max = alt_RISE;
nene_kobayashi 3:507d05fb5cd8 65 count_RISE++;
nene_kobayashi 3:507d05fb5cd8 66 RISEtime = Time.read();
nene_kobayashi 3:507d05fb5cd8 67 }
nene_kobayashi 3:507d05fb5cd8 68 if(Time.read() >= RISEtime + 0.5){
nene_kobayashi 3:507d05fb5cd8 69 count_RISE = 0;
nene_kobayashi 3:507d05fb5cd8 70 }
nene_kobayashi 3:507d05fb5cd8 71 if(count_RISE==5);{
nene_kobayashi 3:507d05fb5cd8 72 servo1.pulsewidth(0.0015);
nene_kobayashi 3:507d05fb5cd8 73 servo2.pulsewidth(0.0015);
nene_kobayashi 3:507d05fb5cd8 74 wait(2.0);
nene_kobayashi 3:507d05fb5cd8 75 myled2 = 1;
nene_kobayashi 2:3a7c38279b0b 76 Phase=DROP1;
nene_kobayashi 2:3a7c38279b0b 77 }
nene_kobayashi 2:3a7c38279b0b 78 break;
nene_kobayashi 2:3a7c38279b0b 79
nene_kobayashi 2:3a7c38279b0b 80 case DROP1:
nene_kobayashi 3:507d05fb5cd8 81 myled3=0;
nene_kobayashi 3:507d05fb5cd8 82 float alt_DROP1 = ms.getAltitude();
nene_kobayashi 3:507d05fb5cd8 83 if(alt_DROP1 - START_koudo < P_openkoudo){
nene_kobayashi 3:507d05fb5cd8 84 count_DROP1++;
nene_kobayashi 3:507d05fb5cd8 85 }
nene_kobayashi 3:507d05fb5cd8 86 if(Time.read() <= DROP1time + 0.5){
nene_kobayashi 3:507d05fb5cd8 87 count_DROP1 = 0;
nene_kobayashi 3:507d05fb5cd8 88 }
nene_kobayashi 3:507d05fb5cd8 89 if(count_DROP1==5){
nene_kobayashi 3:507d05fb5cd8 90 servo3.pulsewidth(0.0015);
nene_kobayashi 3:507d05fb5cd8 91 wait(2.0);
nene_kobayashi 3:507d05fb5cd8 92 myled3 = 1;
nene_kobayashi 3:507d05fb5cd8 93 Phase=DROP2;
nene_kobayashi 2:3a7c38279b0b 94 }
nene_kobayashi 2:3a7c38279b0b 95 break;
nene_kobayashi 3:507d05fb5cd8 96
nene_kobayashi 3:507d05fb5cd8 97 case DROP2:
nene_kobayashi 3:507d05fb5cd8 98 if(ms.getAltitude()== START_koudo){
nene_kobayashi 3:507d05fb5cd8 99 //ここでサーボ電源切る卍卍//
nene_kobayashi 3:507d05fb5cd8 100 Phase=LAND;
nene_kobayashi 3:507d05fb5cd8 101 }
nene_kobayashi 3:507d05fb5cd8 102 break;
nene_kobayashi 3:507d05fb5cd8 103
nene_kobayashi 3:507d05fb5cd8 104
nene_kobayashi 3:507d05fb5cd8 105 case LAND:
nene_kobayashi 3:507d05fb5cd8 106 break;
nene_kobayashi 3:507d05fb5cd8 107 }
nene_kobayashi 2:3a7c38279b0b 108 }
nene_kobayashi 1:fa1980f9b2d0 109
nene_kobayashi 2:3a7c38279b0b 110 int main(){
nene_kobayashi 3:507d05fb5cd8 111 Timer start();
nene_kobayashi 3:507d05fb5cd8 112 Time.start();
nene_kobayashi 3:507d05fb5cd8 113 tic.attach(&_judge,0.1);
nene_kobayashi 2:3a7c38279b0b 114 while(1);
nene_kobayashi 2:3a7c38279b0b 115 return 0;
nene_kobayashi 2:3a7c38279b0b 116 }