2017年能代開放用プログラム
Dependencies: mbed MPU6050 MS5607
main.cpp@1:68309ba0103d, 2017-08-11 (annotated)
- Committer:
- kouki728
- Date:
- Fri Aug 11 13:57:14 2017 +0000
- Revision:
- 1:68309ba0103d
- Parent:
- 0:a4734a842395
advanced work
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kouki728 | 0:a4734a842395 | 1 | #include "mbed.h" |
kouki728 | 1:68309ba0103d | 2 | #include "math.h" |
kouki728 | 1:68309ba0103d | 3 | #include "MPU6050.h" |
kouki728 | 1:68309ba0103d | 4 | #include "MS5607I2C.h" |
kouki728 | 1:68309ba0103d | 5 | |
kouki728 | 1:68309ba0103d | 6 | #define STANBY |
kouki728 | 1:68309ba0103d | 7 | #define LAUNCH |
kouki728 | 1:68309ba0103d | 8 | #define RISE |
kouki728 | 1:68309ba0103d | 9 | #define DROP |
kouki728 | 1:68309ba0103d | 10 | #define ACC |
kouki728 | 1:68309ba0103d | 11 | #define SERCH_TIME |
kouki728 | 1:68309ba0103d | 12 | #define |
kouki728 | 1:68309ba0103d | 13 | #define |
kouki728 | 1:68309ba0103d | 14 | #define |
kouki728 | 1:68309ba0103d | 15 | #define |
kouki728 | 0:a4734a842395 | 16 | |
kouki728 | 1:68309ba0103d | 17 | |
kouki728 | 1:68309ba0103d | 18 | MPU6050 mpu(p9, p10); |
kouki728 | 1:68309ba0103d | 19 | MS5607I2C ms(p9, p10, false); |
kouki728 | 1:68309ba0103d | 20 | PwmOut servo1(p26); |
kouki728 | 1:68309ba0103d | 21 | PwmOut servo2(p25); |
kouki728 | 1:68309ba0103d | 22 | PwmOut servo3(p24); |
kouki728 | 1:68309ba0103d | 23 | DegitalOut myled1(LED1); |
kouki728 | 1:68309ba0103d | 24 | DegitalOut myled2(LED2); |
kouki728 | 1:68309ba0103d | 25 | DegitalOut myled3(LED3); |
kouki728 | 1:68309ba0103d | 26 | Timer timer; |
kouki728 | 1:68309ba0103d | 27 | Ticker ticker; |
kouki728 | 1:68309ba0103d | 28 | |
kouki728 | 1:68309ba0103d | 29 | |
kouki728 | 1:68309ba0103d | 30 | void _judge(); |
kouki728 | 1:68309ba0103d | 31 | float _median(float[5]); |
kouki728 | 1:68309ba0103d | 32 | |
kouki728 | 0:a4734a842395 | 33 | |
kouki728 | 0:a4734a842395 | 34 | int main() { |
kouki728 | 1:68309ba0103d | 35 | timer.start() |
kouki728 | 0:a4734a842395 | 36 | } |
kouki728 | 1:68309ba0103d | 37 | |
kouki728 | 1:68309ba0103d | 38 | void _judge(){ |
kouki728 | 1:68309ba0103d | 39 | switch(FHASE){ |
kouki728 | 1:68309ba0103d | 40 | case STANBY: |
kouki728 | 1:68309ba0103d | 41 | break; |
kouki728 | 1:68309ba0103d | 42 | case LAUNCH: |
kouki728 | 1:68309ba0103d | 43 | myled1 = 0; |
kouki728 | 1:68309ba0103d | 44 | float acc[3], acc_3; |
kouki728 | 1:68309ba0103d | 45 | int count = 0; |
kouki728 | 1:68309ba0103d | 46 | |
kouki728 | 1:68309ba0103d | 47 | getAccelero(acc); |
kouki728 | 1:68309ba0103d | 48 | acc_3 = pow(acc[0], 2) + pow(acc[1], 2) + pow(acc[2], 2); |
kouki728 | 1:68309ba0103d | 49 | acc_3 = sqrt(acc_3); |
kouki728 | 1:68309ba0103d | 50 | |
kouki728 | 1:68309ba0103d | 51 | if(acc >= ACC){ |
kouki728 | 1:68309ba0103d | 52 | if(count == 0) SERCH_TIME = timer.read(); |
kouki728 | 1:68309ba0103d | 53 | count++; |
kouki728 | 1:68309ba0103d | 54 | } |
kouki728 | 1:68309ba0103d | 55 | |
kouki728 | 1:68309ba0103d | 56 | if(timer.read() > time + SERCH_TIME){ |
kouki728 | 1:68309ba0103d | 57 | count = 0; |
kouki728 | 1:68309ba0103d | 58 | } |
kouki728 | 1:68309ba0103d | 59 | |
kouki728 | 1:68309ba0103d | 60 | if(count == COUNT){ |
kouki728 | 1:68309ba0103d | 61 | myled = 1; |
kouki728 | 1:68309ba0103d | 62 | PHASE = RISE; |
kouki728 | 1:68309ba0103d | 63 | } |
kouki728 | 1:68309ba0103d | 64 | |
kouki728 | 1:68309ba0103d | 65 | break; |
kouki728 | 1:68309ba0103d | 66 | case RISE: |
kouki728 | 1:68309ba0103d | 67 | myled2 = 0; |
kouki728 | 1:68309ba0103d | 68 | |
kouki728 | 1:68309ba0103d | 69 | break; |
kouki728 | 1:68309ba0103d | 70 | case DROP: |
kouki728 | 1:68309ba0103d | 71 | |
kouki728 | 1:68309ba0103d | 72 | break; |
kouki728 | 1:68309ba0103d | 73 | |
kouki728 | 1:68309ba0103d | 74 | } |
kouki728 | 1:68309ba0103d | 75 | } |
kouki728 | 1:68309ba0103d | 76 | |
kouki728 | 1:68309ba0103d | 77 | float _median(){ |
kouki728 | 1:68309ba0103d | 78 | |
kouki728 | 1:68309ba0103d | 79 | } |