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?
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 | 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 |