PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_IZU201811_STMBBM_v4

PQ-013 Felix-Luminousの押し出し機構用電装のプログラム おまけで気圧センサのログが取れる

Dependencies:   SDFileSystem mbed pqLPS22HB_lib

main.cpp@0:56dd063c82e9, 2018-11-14 (annotated)

Committer:
Sigma884
Date:
Wed Nov 14 09:42:04 2018 +0000
Revision:
0:56dd063c82e9
PQ-013 Felix-Luminous????????????????; ????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sigma884 0:56dd063c82e9 1 #include "mbed.h"
Sigma884 0:56dd063c82e9 2 #include "pqLPS22HB_lib.h"
Sigma884 0:56dd063c82e9 3 #include "SDFileSystem.h"
Sigma884 0:56dd063c82e9 4 #include "math.h"
Sigma884 0:56dd063c82e9 5
Sigma884 0:56dd063c82e9 6 const float DETECT_ALT = 100.0f;
Sigma884 0:56dd063c82e9 7 const float PUSH_TIME = 5.0f;
Sigma884 0:56dd063c82e9 8
Sigma884 0:56dd063c82e9 9 /*******************************************************************************
Sigma884 0:56dd063c82e9 10 コンストラクタ
Sigma884 0:56dd063c82e9 11 *******************************************************************************/
Sigma884 0:56dd063c82e9 12 Serial pc(USBTX, USBRX, 115200);
Sigma884 0:56dd063c82e9 13 I2C i2c_bus(D4, D5);
Sigma884 0:56dd063c82e9 14 SDFileSystem sd(D11, D12, D13, D10, "sd");
Sigma884 0:56dd063c82e9 15 const char file_name[64] = "/sd/Hybrid_IZU201811_STMBBM_v4.csv";
Sigma884 0:56dd063c82e9 16
Sigma884 0:56dd063c82e9 17 pqLPS22HB_lib LPS33HW(pqLPS22HB_lib::AD0_LOW, i2c_bus);
Sigma884 0:56dd063c82e9 18
Sigma884 0:56dd063c82e9 19 //InterruptIn pushFlight(A0);
Sigma884 0:56dd063c82e9 20 DigitalIn pushFlight(A0);
Sigma884 0:56dd063c82e9 21 DigitalOut led1(D3);
Sigma884 0:56dd063c82e9 22 DigitalOut push(A1);
Sigma884 0:56dd063c82e9 23
Sigma884 0:56dd063c82e9 24 Timeout timeout_stop;
Sigma884 0:56dd063c82e9 25 Ticker tick_push;
Sigma884 0:56dd063c82e9 26 Timer time_main;
Sigma884 0:56dd063c82e9 27
Sigma884 0:56dd063c82e9 28 /*******************************************************************************
Sigma884 0:56dd063c82e9 29 関数のプロトタイプ宣言
Sigma884 0:56dd063c82e9 30 *******************************************************************************/
Sigma884 0:56dd063c82e9 31 void setup();
Sigma884 0:56dd063c82e9 32
Sigma884 0:56dd063c82e9 33 void readSensor();
Sigma884 0:56dd063c82e9 34 void detectSeparate();
Sigma884 0:56dd063c82e9 35
Sigma884 0:56dd063c82e9 36 void printData();
Sigma884 0:56dd063c82e9 37 void readPC();
Sigma884 0:56dd063c82e9 38
Sigma884 0:56dd063c82e9 39 void flightOn();
Sigma884 0:56dd063c82e9 40 void pushChange();
Sigma884 0:56dd063c82e9 41 void stopAll();
Sigma884 0:56dd063c82e9 42
Sigma884 0:56dd063c82e9 43 void startRec();
Sigma884 0:56dd063c82e9 44 void stopRec();
Sigma884 0:56dd063c82e9 45 void recData();
Sigma884 0:56dd063c82e9 46
Sigma884 0:56dd063c82e9 47 /*******************************************************************************
Sigma884 0:56dd063c82e9 48 変数の宣言
Sigma884 0:56dd063c82e9 49 *******************************************************************************/
Sigma884 0:56dd063c82e9 50 FILE *fp;
Sigma884 0:56dd063c82e9 51 bool save = false;
Sigma884 0:56dd063c82e9 52 int write_c = 0;
Sigma884 0:56dd063c82e9 53
Sigma884 0:56dd063c82e9 54 float pres33, temp33, alt33, pres33_0, temp33_0;
Sigma884 0:56dd063c82e9 55 bool pt33_0_set = false;
Sigma884 0:56dd063c82e9 56
Sigma884 0:56dd063c82e9 57 bool detect = false;
Sigma884 0:56dd063c82e9 58 bool pushing = false;
Sigma884 0:56dd063c82e9 59 bool flight = false;
Sigma884 0:56dd063c82e9 60
Sigma884 0:56dd063c82e9 61 int time_read, time_reset = 0;
Sigma884 0:56dd063c82e9 62 int time_min, time_sec, time_ms;
Sigma884 0:56dd063c82e9 63
Sigma884 0:56dd063c82e9 64 /*******************************************************************************
Sigma884 0:56dd063c82e9 65 メイン関数
Sigma884 0:56dd063c82e9 66 *******************************************************************************/
Sigma884 0:56dd063c82e9 67 int main() {
Sigma884 0:56dd063c82e9 68 setup();
Sigma884 0:56dd063c82e9 69
Sigma884 0:56dd063c82e9 70 pc.attach(&readPC, Serial::RxIrq);
Sigma884 0:56dd063c82e9 71
Sigma884 0:56dd063c82e9 72 pushFlight.mode(PullUp);
Sigma884 0:56dd063c82e9 73 //pushFlight.rise(&flightOn);
Sigma884 0:56dd063c82e9 74
Sigma884 0:56dd063c82e9 75 time_main.reset();
Sigma884 0:56dd063c82e9 76 time_main.start();
Sigma884 0:56dd063c82e9 77 startRec();
Sigma884 0:56dd063c82e9 78
Sigma884 0:56dd063c82e9 79 while(1) {
Sigma884 0:56dd063c82e9 80 readSensor();
Sigma884 0:56dd063c82e9 81 printData();
Sigma884 0:56dd063c82e9 82 recData();
Sigma884 0:56dd063c82e9 83 detectSeparate();
Sigma884 0:56dd063c82e9 84 }
Sigma884 0:56dd063c82e9 85 }
Sigma884 0:56dd063c82e9 86
Sigma884 0:56dd063c82e9 87 /*******************************************************************************
Sigma884 0:56dd063c82e9 88 セットアップ（最初に1回実行）
Sigma884 0:56dd063c82e9 89 *******************************************************************************/
Sigma884 0:56dd063c82e9 90 void setup(){
Sigma884 0:56dd063c82e9 91 wait(1.0f);
Sigma884 0:56dd063c82e9 92 char setup_string[512];
Sigma884 0:56dd063c82e9 93
Sigma884 0:56dd063c82e9 94 pc.printf("\r\n**************************************************\r\n");
Sigma884 0:56dd063c82e9 95 pc.printf("Setting Start!!\r\n");
Sigma884 0:56dd063c82e9 96 strcat(setup_string, "\r\n\nSetting Start!!\r\n");
Sigma884 0:56dd063c82e9 97
Sigma884 0:56dd063c82e9 98 ///////////////////////////////////////////LPS33HW
Sigma884 0:56dd063c82e9 99 LPS33HW.begin(50);
Sigma884 0:56dd063c82e9 100 if(LPS33HW.whoAmI() == 0){
Sigma884 0:56dd063c82e9 101 pc.printf("LPS33HW : OK!!\r\n");
Sigma884 0:56dd063c82e9 102 strcat(setup_string, "LPS33HW : OK!!\r\n");
Sigma884 0:56dd063c82e9 103 }
Sigma884 0:56dd063c82e9 104 else{
Sigma884 0:56dd063c82e9 105 pc.printf("LPS33HW : NG...\r\n");
Sigma884 0:56dd063c82e9 106 strcat(setup_string, "LPS33HW : NG...\r\n");
Sigma884 0:56dd063c82e9 107 }
Sigma884 0:56dd063c82e9 108
Sigma884 0:56dd063c82e9 109 led1 = 0;
Sigma884 0:56dd063c82e9 110 push = 0;
Sigma884 0:56dd063c82e9 111
Sigma884 0:56dd063c82e9 112 pc.printf("Setting Finished!!\r\n");
Sigma884 0:56dd063c82e9 113 strcat(setup_string, "Setting Finished!!\r\n");
Sigma884 0:56dd063c82e9 114
Sigma884 0:56dd063c82e9 115 fp = fopen(file_name, "a");
Sigma884 0:56dd063c82e9 116 fprintf(fp, setup_string);
Sigma884 0:56dd063c82e9 117 fclose(fp);
Sigma884 0:56dd063c82e9 118 setup_string[0] = NULL;
Sigma884 0:56dd063c82e9 119
Sigma884 0:56dd063c82e9 120 wait(1.0f);
Sigma884 0:56dd063c82e9 121 }
Sigma884 0:56dd063c82e9 122
Sigma884 0:56dd063c82e9 123 /*******************************************************************************
Sigma884 0:56dd063c82e9 124 センサー読み取り
Sigma884 0:56dd063c82e9 125 *******************************************************************************/
Sigma884 0:56dd063c82e9 126 void readSensor(){
Sigma884 0:56dd063c82e9 127 pres33 = LPS33HW.getPres();
Sigma884 0:56dd063c82e9 128 temp33 = LPS33HW.getTemp();
Sigma884 0:56dd063c82e9 129 alt33 = LPS33HW.getAlt(pres33_0, temp33_0);
Sigma884 0:56dd063c82e9 130 if(!pt33_0_set && pres33 > 900){
Sigma884 0:56dd063c82e9 131 pres33_0 = pres33;
Sigma884 0:56dd063c82e9 132 temp33_0 = temp33;
Sigma884 0:56dd063c82e9 133 pt33_0_set = true;
Sigma884 0:56dd063c82e9 134 }
Sigma884 0:56dd063c82e9 135
Sigma884 0:56dd063c82e9 136 time_read = time_main.read_ms();
Sigma884 0:56dd063c82e9 137 if(time_read >= 30*60*1000){
Sigma884 0:56dd063c82e9 138 time_main.reset();
Sigma884 0:56dd063c82e9 139 time_reset ++;
Sigma884 0:56dd063c82e9 140 }
Sigma884 0:56dd063c82e9 141 time_min = time_reset * 30 + floor((double)time_read / (60 * 1000));
Sigma884 0:56dd063c82e9 142 time_sec = time_read % (60 * 1000);
Sigma884 0:56dd063c82e9 143 time_sec = floor((double)time_sec / 1000);
Sigma884 0:56dd063c82e9 144 time_ms = time_read % 1000;
Sigma884 0:56dd063c82e9 145
Sigma884 0:56dd063c82e9 146 flight = pushFlight;
Sigma884 0:56dd063c82e9 147 }
Sigma884 0:56dd063c82e9 148
Sigma884 0:56dd063c82e9 149 /*******************************************************************************
Sigma884 0:56dd063c82e9 150 押し出し作動条件
Sigma884 0:56dd063c82e9 151 alt33 > DETECT_ALT
Sigma884 0:56dd063c82e9 152 pushFlight == HIGH
Sigma884 0:56dd063c82e9 153 *******************************************************************************/
Sigma884 0:56dd063c82e9 154 void detectSeparate(){
Sigma884 0:56dd063c82e9 155 if(!detect && alt33 > DETECT_ALT && flight){
Sigma884 0:56dd063c82e9 156 flightOn();
Sigma884 0:56dd063c82e9 157 }
Sigma884 0:56dd063c82e9 158 }
Sigma884 0:56dd063c82e9 159
Sigma884 0:56dd063c82e9 160 /*******************************************************************************
Sigma884 0:56dd063c82e9 161 データの表示
Sigma884 0:56dd063c82e9 162 *******************************************************************************/
Sigma884 0:56dd063c82e9 163 void printData(){
Sigma884 0:56dd063c82e9 164 pc.printf("Time : %d:%02d.%03d ", time_min, time_sec, time_ms);
Sigma884 0:56dd063c82e9 165 pc.printf("LPS33HW : %.4f[hPa], %.2f[degC], %.2f[m] ", pres33, temp33, alt33);
Sigma884 0:56dd063c82e9 166 pc.printf("Save : %d, Flight : %d, Detect : %d, Push : %d\r\n", save, flight, detect, pushing);
Sigma884 0:56dd063c82e9 167 }
Sigma884 0:56dd063c82e9 168
Sigma884 0:56dd063c82e9 169 /*******************************************************************************
Sigma884 0:56dd063c82e9 170 PCからの読み取り
Sigma884 0:56dd063c82e9 171 *******************************************************************************/
Sigma884 0:56dd063c82e9 172 void readPC(){
Sigma884 0:56dd063c82e9 173 char c = pc.getc();
Sigma884 0:56dd063c82e9 174 pc.printf("Input : %c\r\n", c);
Sigma884 0:56dd063c82e9 175 wait(0.25f);
Sigma884 0:56dd063c82e9 176
Sigma884 0:56dd063c82e9 177 switch(c){
Sigma884 0:56dd063c82e9 178 case 'r':
Sigma884 0:56dd063c82e9 179 pres33_0 = pres33;
Sigma884 0:56dd063c82e9 180 temp33_0 = temp33;
Sigma884 0:56dd063c82e9 181 break;
Sigma884 0:56dd063c82e9 182 }
Sigma884 0:56dd063c82e9 183 }
Sigma884 0:56dd063c82e9 184
Sigma884 0:56dd063c82e9 185 /*******************************************************************************
Sigma884 0:56dd063c82e9 186 フライトピン
Sigma884 0:56dd063c82e9 187 *******************************************************************************/
Sigma884 0:56dd063c82e9 188 void flightOn(){
Sigma884 0:56dd063c82e9 189 if(!detect){
Sigma884 0:56dd063c82e9 190 pc.printf("Flight ON!!!!\r\n");
Sigma884 0:56dd063c82e9 191 tick_push.attach(&pushChange, 0.5f);
Sigma884 0:56dd063c82e9 192 timeout_stop.attach(&stopAll, PUSH_TIME);
Sigma884 0:56dd063c82e9 193 led1 = 1;
Sigma884 0:56dd063c82e9 194 detect = true;
Sigma884 0:56dd063c82e9 195 }
Sigma884 0:56dd063c82e9 196 }
Sigma884 0:56dd063c82e9 197
Sigma884 0:56dd063c82e9 198 void pushChange(){
Sigma884 0:56dd063c82e9 199 if(push == 0){
Sigma884 0:56dd063c82e9 200 push = 1;
Sigma884 0:56dd063c82e9 201 pushing = true;
Sigma884 0:56dd063c82e9 202 }
Sigma884 0:56dd063c82e9 203 else{
Sigma884 0:56dd063c82e9 204 push = 0;
Sigma884 0:56dd063c82e9 205 pushing = false;
Sigma884 0:56dd063c82e9 206 }
Sigma884 0:56dd063c82e9 207 }
Sigma884 0:56dd063c82e9 208
Sigma884 0:56dd063c82e9 209 void stopAll(){
Sigma884 0:56dd063c82e9 210 tick_push.detach();
Sigma884 0:56dd063c82e9 211 push = 0;
Sigma884 0:56dd063c82e9 212 pushing = false;
Sigma884 0:56dd063c82e9 213
Sigma884 0:56dd063c82e9 214 stopRec();
Sigma884 0:56dd063c82e9 215 time_main.stop();
Sigma884 0:56dd063c82e9 216 }
Sigma884 0:56dd063c82e9 217
Sigma884 0:56dd063c82e9 218 /*******************************************************************************
Sigma884 0:56dd063c82e9 219 記録開始
Sigma884 0:56dd063c82e9 220 *******************************************************************************/
Sigma884 0:56dd063c82e9 221 void startRec(){
Sigma884 0:56dd063c82e9 222 fp = fopen(file_name, "a");
Sigma884 0:56dd063c82e9 223 fprintf(fp, "time,pres[hPa],temp[degC],alt[m],detect,push,flight\r\n");
Sigma884 0:56dd063c82e9 224 save = true;
Sigma884 0:56dd063c82e9 225 }
Sigma884 0:56dd063c82e9 226
Sigma884 0:56dd063c82e9 227 /*******************************************************************************
Sigma884 0:56dd063c82e9 228 記録終了
Sigma884 0:56dd063c82e9 229 *******************************************************************************/
Sigma884 0:56dd063c82e9 230 void stopRec(){
Sigma884 0:56dd063c82e9 231 save = false;
Sigma884 0:56dd063c82e9 232 fprintf(fp, "\r\n\n");
Sigma884 0:56dd063c82e9 233 fclose(fp);
Sigma884 0:56dd063c82e9 234 }
Sigma884 0:56dd063c82e9 235
Sigma884 0:56dd063c82e9 236 /*******************************************************************************
Sigma884 0:56dd063c82e9 237 データを記録
Sigma884 0:56dd063c82e9 238 *******************************************************************************/
Sigma884 0:56dd063c82e9 239 void recData(){
Sigma884 0:56dd063c82e9 240 if(save){
Sigma884 0:56dd063c82e9 241 fprintf(fp, "%d:%02d.%03d,", time_min, time_sec, time_ms);
Sigma884 0:56dd063c82e9 242 fprintf(fp, "%.4f,%.2f,%.2f,", pres33, temp33, alt33);
Sigma884 0:56dd063c82e9 243 fprintf(fp, "%d,%d,%d\r\n", detect, pushing, flight);
Sigma884 0:56dd063c82e9 244
Sigma884 0:56dd063c82e9 245 write_c ++;
Sigma884 0:56dd063c82e9 246 if(write_c > 1000){
Sigma884 0:56dd063c82e9 247 fclose(fp);
Sigma884 0:56dd063c82e9 248 fp = fopen(file_name, "a");
Sigma884 0:56dd063c82e9 249 write_c = 0;
Sigma884 0:56dd063c82e9 250 }
Sigma884 0:56dd063c82e9 251 }
Sigma884 0:56dd063c82e9 252 }