CORE / Mbed 2 deprecated 2017_03_Kaiho

2017年3月，伊豆大島共同打上実験 CORE＿缶ロケチーム電装

Dependencies:   MPU6050 MS5607 mbed SDFileSystem

main.cpp@9:ac43a90b6afb, 2017-02-23 (annotated)

Committer:
mikawataru
Date:
Thu Feb 23 14:59:45 2017 +0000
Branch:
switchUI
Revision:
9:ac43a90b6afb
Parent:
8:a9e115560ba3 
add switch of servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 5:bcf55d8fe7a7 1 /*
mikawataru 5:bcf55d8fe7a7 2 2017年3月　伊豆大島共同打上実験
mikawataru 5:bcf55d8fe7a7 3 団体名：CORE
mikawataru 5:bcf55d8fe7a7 4 チーム名：ヌペリオル
mikawataru 5:bcf55d8fe7a7 5 該当電装：ロケット搭載用
mikawataru 5:bcf55d8fe7a7 6
mikawataru 5:bcf55d8fe7a7 7 使用部品
mikawataru 5:bcf55d8fe7a7 8 ・LPC1768(マイコン)
mikawataru 5:bcf55d8fe7a7 9 ・MPU6050(加速度・ジャイロセンサ)
mikawataru 5:bcf55d8fe7a7 10 ・MS5607(気圧・気温センサ)
mikawataru 5:bcf55d8fe7a7 11 ・MicroSDスロット
mikawataru 5:bcf55d8fe7a7 12 ・MG995(サーボモータ)×4
mikawataru 5:bcf55d8fe7a7 13
mikawataru 7:5f693667d0e9 14 使用ライブラリ
mikawataru 7:5f693667d0e9 15 ・MPU6050.h
mikawataru 7:5f693667d0e9 16 https://developer.mbed.org/teams/mbed/code/SDFileSystem/
mikawataru 7:5f693667d0e9 17 ・MS5607.h
mikawataru 7:5f693667d0e9 18 https://developer.mbed.org/users/yamaguch/code/MS5607/
mikawataru 7:5f693667d0e9 19 ・SDFileSystem.h
mikawataru 7:5f693667d0e9 20 https://developer.mbed.org/teams/mbed/code/SDFileSystem/
mikawataru 7:5f693667d0e9 21
mikawataru 5:bcf55d8fe7a7 22 ピン配置
mikawataru 5:bcf55d8fe7a7 23 LPC1768 MPU6050 MS5607 MicroSD　MG995_1 MG995_2 MG995_3 MG995_4
mikawataru 5:bcf55d8fe7a7 24 VIN--------------------------------------------------------------------4.5~9.0V
mikawataru 5:bcf55d8fe7a7 25 -----------------------------------VIN------VIN------VIN------VIN------5V
mikawataru 5:bcf55d8fe7a7 26 Vout-----VCC------VIN-----VDD------------------------------------------3.3V
mikawataru 6:15204813ef87 27 \-ADO \-PS
mikawataru 5:bcf55d8fe7a7 28 GND------GND------GND-----VSS------GND------GND------GND------GND------0V
mikawataru 6:15204813ef87 29 \-CS
mikawataru 5:bcf55d8fe7a7 30 p9-------SDA------SDA----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 31 p10------SCL------SCL----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 32 p11-----------------------CLK--------------------------------------------
mikawataru 5:bcf55d8fe7a7 33 p12-----------------------DAT0-------------------------------------------
mikawataru 5:bcf55d8fe7a7 34 p13-----------------------CMD--------------------------------------------
mikawataru 5:bcf55d8fe7a7 35 p14-----------------------CD/DAT0----------------------------------------
mikawataru 5:bcf55d8fe7a7 36 p23--------------------------------CTRL----------------------------------
mikawataru 5:bcf55d8fe7a7 37 p24-----------------------------------------CTRL-------------------------
mikawataru 5:bcf55d8fe7a7 38 p25--------------------------------------------------CTRL----------------
mikawataru 5:bcf55d8fe7a7 39 p26-----------------------------------------------------------CTRL-------
mikawataru 5:bcf55d8fe7a7 40 */
mikawataru 0:10a9c9c5ce83 41 #include "mbed.h"
mikawataru 7:5f693667d0e9 42 #include "math.h"
mikawataru 0:10a9c9c5ce83 43 #include "MS5607I2C.h"
mikawataru 0:10a9c9c5ce83 44 #include "MPU6050.h"
mikawataru 1:ec75f428c6b3 45 #include "SDFileSystem.h"
mikawataru 7:5f693667d0e9 46 /*判定パラメータ*/
mikawataru 8:a9e115560ba3 47 #define ACC_LAUNCH 4//発射判定加速度[g]
mikawataru 7:5f693667d0e9 48 #define DROP_TOP 1.5//頂点判定最低降下量[m]
mikawataru 8:a9e115560ba3 49 #define ALT_CAN 350//缶サット開放高度[m]
mikawataru 8:a9e115560ba3 50 #define TIME_TOP 15//タイマー開放時間[s]
mikawataru 3:9cd74af355cc 51 /*動作レート*/
mikawataru 3:9cd74af355cc 52 #define RATE_LOG 10//ログ用
mikawataru 3:9cd74af355cc 53 #define RATE_OPEN 10//開放用
mikawataru 3:9cd74af355cc 54 /*サーボ動作*/
mikawataru 6:15204813ef87 55 #define LOCK 0.0005
mikawataru 6:15204813ef87 56 #define UNLOCK 0.0015
mikawataru 3:9cd74af355cc 57 /*フェイズ定義*/
mikawataru 3:9cd74af355cc 58 #define STANDBY 0
mikawataru 3:9cd74af355cc 59 #define LAUNCH 1
mikawataru 3:9cd74af355cc 60 #define FLIGHT 2
mikawataru 7:5f693667d0e9 61 #define DROP 3
mikawataru 0:10a9c9c5ce83 62
mikawataru 8:a9e115560ba3 63 #define P0 1013.25f//海面気圧[hPa]
mikawataru 7:5f693667d0e9 64 /*ピン指定*/
mikawataru 0:10a9c9c5ce83 65 MS5607I2C ms5607(p9, p10, false);
mikawataru 0:10a9c9c5ce83 66 MPU6050 mpu(p9,p10);
mikawataru 0:10a9c9c5ce83 67 DigitalIn sw(p21);
mikawataru 9:ac43a90b6afb 68 DigitalIn sw_servo_1(p19);
mikawataru 9:ac43a90b6afb 69 DigitalIn sw_servo_1(p20);
mikawataru 3:9cd74af355cc 70 BusOut myled(LED1,LED2,LED3,LED4);
mikawataru 7:5f693667d0e9 71 SDFileSystem sd(p11, p12, p13, p14, "sd");
mikawataru 0:10a9c9c5ce83 72 Serial pc(USBTX, USBRX);
mikawataru 5:bcf55d8fe7a7 73 PwmOut Door_1_1(p23);
mikawataru 5:bcf55d8fe7a7 74 PwmOut Door_1_2(p24);
mikawataru 5:bcf55d8fe7a7 75 PwmOut Door_2_1(p25);
mikawataru 5:bcf55d8fe7a7 76 PwmOut Door_2_2(p26);
mikawataru 7:5f693667d0e9 77 /*タイマー類*/
mikawataru 1:ec75f428c6b3 78 Timer timer;
mikawataru 3:9cd74af355cc 79 Ticker loop_log;
mikawataru 3:9cd74af355cc 80 Ticker loop_open;
mikawataru 5:bcf55d8fe7a7 81 /*ログカウンタ*/
mikawataru 5:bcf55d8fe7a7 82 bool row = 0;
mikawataru 3:9cd74af355cc 83 int8_t col = 0;
mikawataru 5:bcf55d8fe7a7 84 /*ログ格納用*/
mikawataru 3:9cd74af355cc 85 float pressure[2][RATE_LOG],temperature[2][RATE_LOG];
mikawataru 3:9cd74af355cc 86 float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
mikawataru 3:9cd74af355cc 87 float t[2][RATE_LOG];
mikawataru 7:5f693667d0e9 88 FILE *fp;
mikawataru 5:bcf55d8fe7a7 89 /*フェイズ変数*/
mikawataru 5:bcf55d8fe7a7 90 int8_t Phase = STANDBY;
mikawataru 7:5f693667d0e9 91 /*判定用*/
mikawataru 8:a9e115560ba3 92 float a_buff;
mikawataru 7:5f693667d0e9 93 float alt,alt_max,alt_launch;
mikawataru 7:5f693667d0e9 94 float t_drop,t_top,t_launch;
mikawataru 7:5f693667d0e9 95 int8_t cnt_launch = 0;
mikawataru 7:5f693667d0e9 96 int8_t cnt_top = 0;
mikawataru 3:9cd74af355cc 97 int8_t cnt_drop = 0;
mikawataru 7:5f693667d0e9 98 /*関数*/
mikawataru 7:5f693667d0e9 99 float getAlt(float press, float temp);
mikawataru 3:9cd74af355cc 100 void _Open();
mikawataru 6:15204813ef87 101 void _Servo(int8_t door_num, float motion);
mikawataru 3:9cd74af355cc 102 void _Log();
mikawataru 7:5f693667d0e9 103 /*---------------------------------------------------------------------*/
mikawataru 0:10a9c9c5ce83 104 int main() {
mikawataru 1:ec75f428c6b3 105 timer.start();
mikawataru 5:bcf55d8fe7a7 106 Door_1_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 107 Door_1_2.period_ms(20);
mikawataru 5:bcf55d8fe7a7 108 Door_2_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 109 Door_2_2.period_ms(20);
mikawataru 6:15204813ef87 110 _Servo(0,LOCK);
mikawataru 2:b6eb08d059cc 111 fp = fopen("/sd/log.txt", "w");
mikawataru 1:ec75f428c6b3 112 fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
mikawataru 3:9cd74af355cc 113 loop_log.attach(&_Log,1.0/RATE_LOG);
mikawataru 3:9cd74af355cc 114 loop_open.attach(&_Open,1.0/RATE_OPEN);
mikawataru 3:9cd74af355cc 115 while(1);
mikawataru 3:9cd74af355cc 116 }
mikawataru 6:15204813ef87 117 /*開放用関数　RATE_OPEN[Hz]で判定を行う*/
mikawataru 3:9cd74af355cc 118 void _Open(){
mikawataru 3:9cd74af355cc 119 myled = 1 << (Phase-1);
mikawataru 3:9cd74af355cc 120 switch (Phase) {
mikawataru 3:9cd74af355cc 121 case STANDBY://スタンバイモード（発射判定不可）
mikawataru 7:5f693667d0e9 122 if(sw==0)Phase = LAUNCH;
mikawataru 3:9cd74af355cc 123 break;
mikawataru 7:5f693667d0e9 124 case LAUNCH://点火モード（発射判定可）
mikawataru 7:5f693667d0e9 125 alt_launch = alt;
mikawataru 7:5f693667d0e9 126 if((-1)*a_buff>ACC_LAUNCH){
mikawataru 7:5f693667d0e9 127 if(cnt_launch++==3) Phase = FLIGHT;
mikawataru 7:5f693667d0e9 128 t_launch = timer.read();
mikawataru 7:5f693667d0e9 129 alt_max = alt;
mikawataru 7:5f693667d0e9 130 }else{
mikawataru 7:5f693667d0e9 131 if(timer.read()>t_launch+1.0) cnt_launch = 0;
mikawataru 3:9cd74af355cc 132 }
mikawataru 4:cc266df87f3e 133 break;
mikawataru 7:5f693667d0e9 134 case FLIGHT://飛翔中（パラシュート開放判定）
mikawataru 7:5f693667d0e9 135 if(alt>alt_max) alt_max = alt;
mikawataru 7:5f693667d0e9 136 if(alt<alt_max-DROP_TOP){
mikawataru 7:5f693667d0e9 137 if(cnt_top++==3){
mikawataru 7:5f693667d0e9 138 _Servo(1,UNLOCK);
mikawataru 7:5f693667d0e9 139 Phase = DROP;
mikawataru 7:5f693667d0e9 140 }
mikawataru 7:5f693667d0e9 141 t_top = timer.read();
mikawataru 7:5f693667d0e9 142 }else{
mikawataru 7:5f693667d0e9 143 if(timer.read()>t_top+1.0) cnt_top = 0;
mikawataru 7:5f693667d0e9 144 }
mikawataru 7:5f693667d0e9 145 if(timer.read()-t_launch>TIME_TOP){
mikawataru 7:5f693667d0e9 146 _Servo(1,UNLOCK);
mikawataru 7:5f693667d0e9 147 Phase = DROP;
mikawataru 7:5f693667d0e9 148 }
mikawataru 7:5f693667d0e9 149 break;
mikawataru 7:5f693667d0e9 150 case DROP://降下中（缶サット開放判定)
mikawataru 7:5f693667d0e9 151 if(alt < alt_launch+ALT_CAN){
mikawataru 7:5f693667d0e9 152 if(cnt_drop++==3)_Servo(2,UNLOCK);
mikawataru 7:5f693667d0e9 153 t_drop = timer.read();
mikawataru 7:5f693667d0e9 154 }else{
mikawataru 7:5f693667d0e9 155 if(timer.read()>t_drop+1.0)cnt_drop = 0;
mikawataru 7:5f693667d0e9 156 }
mikawataru 3:9cd74af355cc 157 break;
mikawataru 0:10a9c9c5ce83 158 }
mikawataru 0:10a9c9c5ce83 159 }
mikawataru 6:15204813ef87 160 /*記録用関数　RATE_LOG[Hz]で記録を行う*/
mikawataru 3:9cd74af355cc 161 void _Log(){
mikawataru 3:9cd74af355cc 162 t[row][col] = timer.read();
mikawataru 7:5f693667d0e9 163 pressure[row][col] = ms5607.getPressure()/100;
mikawataru 3:9cd74af355cc 164 temperature[row][col] = ms5607.getTemperature();
mikawataru 3:9cd74af355cc 165 mpu.getAccelero(&acc[row][col][0]);
mikawataru 3:9cd74af355cc 166 mpu.getGyro(&gyro[row][col][0]);
mikawataru 7:5f693667d0e9 167 a_buff = acc[row][col][2];
mikawataru 7:5f693667d0e9 168 alt = getAlt(pressure[row][col],temperature[row][col]);
mikawataru 3:9cd74af355cc 169 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
mikawataru 3:9cd74af355cc 170 t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0],
mikawataru 3:9cd74af355cc 171 acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
mikawataru 3:9cd74af355cc 172 );
mikawataru 3:9cd74af355cc 173 if(col++==RATE_LOG){
mikawataru 3:9cd74af355cc 174 fclose(fp);
mikawataru 3:9cd74af355cc 175 fp = fopen("/sd/log.txt", "a");
mikawataru 3:9cd74af355cc 176 row =! row;
mikawataru 3:9cd74af355cc 177 col = 0;
mikawataru 3:9cd74af355cc 178 }
mikawataru 3:9cd74af355cc 179 }
mikawataru 6:15204813ef87 180 /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
mikawataru 6:15204813ef87 181 void _Servo(int8_t door_num, float motion){
mikawataru 6:15204813ef87 182 if(door_num==0){//全扉
mikawataru 6:15204813ef87 183 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 184 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 185 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 186 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 187 }else if(door_num==1){//扉１
mikawataru 6:15204813ef87 188 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 189 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 190 }else if(door_num==2){//扉２
mikawataru 6:15204813ef87 191 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 192 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 193 }else{
mikawataru 6:15204813ef87 194 pc.printf("error\r\n");
mikawataru 6:15204813ef87 195 }
mikawataru 3:9cd74af355cc 196 }
mikawataru 7:5f693667d0e9 197
mikawataru 7:5f693667d0e9 198 float getAlt(float press, float temp){
mikawataru 7:5f693667d0e9 199 return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
mikawataru 7:5f693667d0e9 200 }