nene kobayashi / Mbed 2 deprecated 201708kaiho_kobayashi

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

Dependencies:   mbed MS5607 MPU6050

main.cpp@4:a44c7a8adaeb, 2017-08-06 (annotated)

Committer:
nene_kobayashi
Date:
Sun Aug 06 09:48:17 2017 +0000
Revision:
4:a44c7a8adaeb
Parent:
3:507d05fb5cd8
Child:
5:e5c45611bf43
nemui

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