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?
User | Revision | Line number | New 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 | } |