CORE / Mbed 2 deprecated 2017_03_Kaiho

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

Dependencies:   MPU6050 MS5607 mbed SDFileSystem

main.cpp@6:15204813ef87, 2017-02-22 (annotated)

Committer:
mikawataru
Date:
Wed Feb 22 08:49:59 2017 +0000
Revision:
6:15204813ef87
Parent:
5:bcf55d8fe7a7
Child:
7:5f693667d0e9
add motion of servomotor

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 5:bcf55d8fe7a7 14 ピン配置
mikawataru 5:bcf55d8fe7a7 15 LPC1768 MPU6050 MS5607 MicroSD　MG995_1 MG995_2 MG995_3 MG995_4
mikawataru 5:bcf55d8fe7a7 16 VIN--------------------------------------------------------------------4.5~9.0V
mikawataru 5:bcf55d8fe7a7 17 -----------------------------------VIN------VIN------VIN------VIN------5V
mikawataru 5:bcf55d8fe7a7 18 Vout-----VCC------VIN-----VDD------------------------------------------3.3V
mikawataru 6:15204813ef87 19 \-ADO \-PS
mikawataru 5:bcf55d8fe7a7 20 GND------GND------GND-----VSS------GND------GND------GND------GND------0V
mikawataru 6:15204813ef87 21 \-CS
mikawataru 5:bcf55d8fe7a7 22 p9-------SDA------SDA----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 23 p10------SCL------SCL----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 24 p11-----------------------CLK--------------------------------------------
mikawataru 5:bcf55d8fe7a7 25 p12-----------------------DAT0-------------------------------------------
mikawataru 5:bcf55d8fe7a7 26 p13-----------------------CMD--------------------------------------------
mikawataru 5:bcf55d8fe7a7 27 p14-----------------------CD/DAT0----------------------------------------
mikawataru 5:bcf55d8fe7a7 28 p23--------------------------------CTRL----------------------------------
mikawataru 5:bcf55d8fe7a7 29 p24-----------------------------------------CTRL-------------------------
mikawataru 5:bcf55d8fe7a7 30 p25--------------------------------------------------CTRL----------------
mikawataru 5:bcf55d8fe7a7 31 p26-----------------------------------------------------------CTRL-------
mikawataru 5:bcf55d8fe7a7 32 */
mikawataru 5:bcf55d8fe7a7 33
mikawataru 0:10a9c9c5ce83 34 #include "mbed.h"
mikawataru 0:10a9c9c5ce83 35 #include "MS5607I2C.h"
mikawataru 0:10a9c9c5ce83 36 #include "MPU6050.h"
mikawataru 1:ec75f428c6b3 37 #include "SDFileSystem.h"
mikawataru 0:10a9c9c5ce83 38
mikawataru 3:9cd74af355cc 39 /*動作レート*/
mikawataru 3:9cd74af355cc 40 #define RATE_LOG 10//ログ用
mikawataru 3:9cd74af355cc 41 #define RATE_OPEN 10//開放用
mikawataru 3:9cd74af355cc 42 /*サーボ動作*/
mikawataru 6:15204813ef87 43 #define LOCK 0.0005
mikawataru 6:15204813ef87 44 #define UNLOCK 0.0015
mikawataru 3:9cd74af355cc 45 /*フェイズ定義*/
mikawataru 3:9cd74af355cc 46 #define STANDBY 0
mikawataru 3:9cd74af355cc 47 #define LAUNCH 1
mikawataru 3:9cd74af355cc 48 #define FLIGHT 2
mikawataru 3:9cd74af355cc 49 #define OPEN_1 3
mikawataru 3:9cd74af355cc 50 #define OPEN_2 4
mikawataru 0:10a9c9c5ce83 51
mikawataru 0:10a9c9c5ce83 52 MS5607I2C ms5607(p9, p10, false);
mikawataru 0:10a9c9c5ce83 53 MPU6050 mpu(p9,p10);
mikawataru 0:10a9c9c5ce83 54 DigitalIn sw(p21);
mikawataru 3:9cd74af355cc 55 BusOut myled(LED1,LED2,LED3,LED4);
mikawataru 0:10a9c9c5ce83 56 Serial pc(USBTX, USBRX);
mikawataru 1:ec75f428c6b3 57 SDFileSystem sd(p11, p12, p13, p14, "sd");
mikawataru 0:10a9c9c5ce83 58
mikawataru 5:bcf55d8fe7a7 59 PwmOut Door_1_1(p23);
mikawataru 5:bcf55d8fe7a7 60 PwmOut Door_1_2(p24);
mikawataru 5:bcf55d8fe7a7 61 PwmOut Door_2_1(p25);
mikawataru 5:bcf55d8fe7a7 62 PwmOut Door_2_2(p26);
mikawataru 5:bcf55d8fe7a7 63
mikawataru 1:ec75f428c6b3 64 Timer timer;
mikawataru 3:9cd74af355cc 65 Ticker loop_log;
mikawataru 3:9cd74af355cc 66 Ticker loop_open;
mikawataru 0:10a9c9c5ce83 67
mikawataru 5:bcf55d8fe7a7 68 /*ログカウンタ*/
mikawataru 5:bcf55d8fe7a7 69 bool row = 0;
mikawataru 3:9cd74af355cc 70 int8_t col = 0;
mikawataru 5:bcf55d8fe7a7 71 /*ログ格納用*/
mikawataru 3:9cd74af355cc 72 float pressure[2][RATE_LOG],temperature[2][RATE_LOG];
mikawataru 3:9cd74af355cc 73 float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
mikawataru 3:9cd74af355cc 74 float t[2][RATE_LOG];
mikawataru 3:9cd74af355cc 75 float t_top,alt,alt_max,alt_launch;
mikawataru 5:bcf55d8fe7a7 76 /*フェイズ変数*/
mikawataru 5:bcf55d8fe7a7 77 int8_t Phase = STANDBY;
mikawataru 5:bcf55d8fe7a7 78 /*降下判定用*/
mikawataru 3:9cd74af355cc 79 int8_t cnt_drop = 0;
mikawataru 1:ec75f428c6b3 80 FILE *fp;
mikawataru 1:ec75f428c6b3 81
mikawataru 3:9cd74af355cc 82 void _Open();
mikawataru 6:15204813ef87 83 void _Servo(int8_t door_num, float motion);
mikawataru 3:9cd74af355cc 84 void _Log();
mikawataru 0:10a9c9c5ce83 85
mikawataru 0:10a9c9c5ce83 86 int main() {
mikawataru 1:ec75f428c6b3 87 timer.start();
mikawataru 5:bcf55d8fe7a7 88 Door_1_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 89 Door_1_2.period_ms(20);
mikawataru 5:bcf55d8fe7a7 90 Door_2_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 91 Door_2_2.period_ms(20);
mikawataru 6:15204813ef87 92 _Servo(0,LOCK);
mikawataru 2:b6eb08d059cc 93 fp = fopen("/sd/log.txt", "w");
mikawataru 1:ec75f428c6b3 94 fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
mikawataru 3:9cd74af355cc 95 loop_log.attach(&_Log,1.0/RATE_LOG);
mikawataru 3:9cd74af355cc 96 loop_open.attach(&_Open,1.0/RATE_OPEN);
mikawataru 3:9cd74af355cc 97 while(1);
mikawataru 3:9cd74af355cc 98 }
mikawataru 3:9cd74af355cc 99
mikawataru 6:15204813ef87 100 /*開放用関数　RATE_OPEN[Hz]で判定を行う*/
mikawataru 3:9cd74af355cc 101 void _Open(){
mikawataru 3:9cd74af355cc 102 myled = 1 << (Phase-1);
mikawataru 3:9cd74af355cc 103 switch (Phase) {
mikawataru 3:9cd74af355cc 104 case STANDBY://スタンバイモード（発射判定不可）
mikawataru 3:9cd74af355cc 105 Phase = LAUNCH;
mikawataru 3:9cd74af355cc 106 break;
mikawataru 3:9cd74af355cc 107 case LAUNCH://フライトモード（発射判定可）
mikawataru 5:bcf55d8fe7a7 108 alt_launch = ms5607.getAltitude();
mikawataru 3:9cd74af355cc 109 if(sw==0)Phase = FLIGHT;
mikawataru 3:9cd74af355cc 110 break;
mikawataru 3:9cd74af355cc 111 case FLIGHT://飛翔中（頂点判定可）
mikawataru 3:9cd74af355cc 112 alt=ms5607.getAltitude();
mikawataru 3:9cd74af355cc 113 if(alt > alt_max) alt_max = alt;
mikawataru 3:9cd74af355cc 114 else if(alt_max - alt > 1.5) Phase = OPEN_1;
mikawataru 3:9cd74af355cc 115 t_top = timer.read();
mikawataru 3:9cd74af355cc 116 break;
mikawataru 3:9cd74af355cc 117 case OPEN_1://パラシュート開放モード
mikawataru 3:9cd74af355cc 118 if(alt_max>1.5) cnt_drop++;
mikawataru 3:9cd74af355cc 119 if(cnt_drop==5){
mikawataru 3:9cd74af355cc 120 _Servo(1,UNLOCK);
mikawataru 3:9cd74af355cc 121 Phase = OPEN_2;
mikawataru 3:9cd74af355cc 122 cnt_drop = 0;
mikawataru 3:9cd74af355cc 123 }
mikawataru 4:cc266df87f3e 124 break;
mikawataru 3:9cd74af355cc 125 case OPEN_2://缶サット開放モード
mikawataru 3:9cd74af355cc 126 if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++;
mikawataru 3:9cd74af355cc 127 if (cnt_drop==5) _Servo(2,UNLOCK);
mikawataru 3:9cd74af355cc 128 break;
mikawataru 0:10a9c9c5ce83 129 }
mikawataru 0:10a9c9c5ce83 130 }
mikawataru 3:9cd74af355cc 131
mikawataru 6:15204813ef87 132 /*記録用関数　RATE_LOG[Hz]で記録を行う*/
mikawataru 3:9cd74af355cc 133 void _Log(){
mikawataru 3:9cd74af355cc 134 t[row][col] = timer.read();
mikawataru 3:9cd74af355cc 135 pressure[row][col] = ms5607.getPressure();
mikawataru 3:9cd74af355cc 136 temperature[row][col] = ms5607.getTemperature();
mikawataru 3:9cd74af355cc 137 mpu.getAccelero(&acc[row][col][0]);
mikawataru 3:9cd74af355cc 138 mpu.getGyro(&gyro[row][col][0]);
mikawataru 3:9cd74af355cc 139 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
mikawataru 3:9cd74af355cc 140 t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0],
mikawataru 3:9cd74af355cc 141 acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
mikawataru 3:9cd74af355cc 142 );
mikawataru 3:9cd74af355cc 143 if(col++==RATE_LOG){
mikawataru 3:9cd74af355cc 144 fclose(fp);
mikawataru 3:9cd74af355cc 145 fp = fopen("/sd/log.txt", "a");
mikawataru 3:9cd74af355cc 146 row =! row;
mikawataru 3:9cd74af355cc 147 col = 0;
mikawataru 3:9cd74af355cc 148 }
mikawataru 3:9cd74af355cc 149 }
mikawataru 3:9cd74af355cc 150
mikawataru 6:15204813ef87 151 /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
mikawataru 6:15204813ef87 152 void _Servo(int8_t door_num, float motion){
mikawataru 6:15204813ef87 153 if(door_num==0){//全扉
mikawataru 6:15204813ef87 154 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 155 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 156 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 157 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 158 }else if(door_num==1){//扉１
mikawataru 6:15204813ef87 159 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 160 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 161 }else if(door_num==2){//扉２
mikawataru 6:15204813ef87 162 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 163 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 164 }else{
mikawataru 6:15204813ef87 165 pc.printf("error\r\n");
mikawataru 6:15204813ef87 166 }
mikawataru 3:9cd74af355cc 167 }