Revision 3:9cd74af355cc, committed 2017-02-18

Comitter:

mikawataru

Date:

Sat Feb 18 19:08:20 2017 +0000

Parent:

2:b6eb08d059cc

Child:

4:cc266df87f3e

Commit message:

?????????????????

Changed in this revision

--- a/main.cpp	Sat Feb 18 17:32:44 2017 +0000
+++ b/main.cpp	Sat Feb 18 19:08:20 2017 +0000
@@ -3,57 +3,101 @@
 #include "MPU6050.h"
 #include "SDFileSystem.h"
 
-#define RATE 10
+/*動作レート*/
+#define RATE_LOG 10//ログ用
+#define RATE_OPEN 10//開放用
+/*サーボ動作*/
+#define LOCK true
+#define UNLOCK false
+/*フェイズ定義*/
+#define STANDBY 0
+#define LAUNCH 1
+#define FLIGHT 2
+#define OPEN_1 3
+#define OPEN_2 4
 
 MS5607I2C ms5607(p9, p10, false);
 MPU6050 mpu(p9,p10);
 DigitalIn sw(p21);
-DigitalOut myled(LED1);
+BusOut myled(LED1,LED2,LED3,LED4);
 Serial pc(USBTX, USBRX);
 SDFileSystem sd(p11, p12, p13, p14, "sd");
 
 Timer timer;
-Ticker loop_measure;
+Ticker loop_log;
+Ticker loop_open;
 
-int8_t cnt = 0;
-bool mode = 0;
-float pressure[2][RATE],temperature[2][RATE];
-float acc[2][RATE][3],gyro[2][RATE][3];
-float t[2][RATE],alt,alt_max,alt_launch;
+int8_t col = 0;
+int8_t Phase = STANDBY;
+bool row = 0;
+float pressure[2][RATE_LOG],temperature[2][RATE_LOG];
+float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
+float t[2][RATE_LOG];
+float t_top,alt,alt_max,alt_launch;
+int8_t cnt_drop = 0;
 FILE *fp;
 
-void _Launch(){
-   alt_launch = alt;
-   myled = 1;
-
-}
-void _measure(){
-   t[mode][cnt] = timer.read();
-   pressure[mode][cnt] = ms5607.getPressure();
-   temperature[mode][cnt] = ms5607.getTemperature();
-   mpu.getAccelero(&acc[mode][cnt][0]);
-   mpu.getGyro(&gyro[mode][cnt][0]);
-   alt = ms5607.getAltitude();
-   fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
-               t[mode][cnt],pressure[mode][cnt],temperature[mode][cnt],acc[mode][cnt][0],
-               acc[mode][cnt][1],acc[mode][cnt][2],gyro[mode][cnt][0],gyro[mode][cnt][1],gyro[mode][cnt][2]
-            );
-   cnt++;
-   if(cnt==RATE){
-      fclose(fp);
-      fp = fopen("/sd/log.txt", "a");
-      mode =! mode;
-      cnt = 0;
-    }
-}
+void _Open();
+void _Servo(int8_t door_num, bool door_motion);
+void _Log();
 
 int main() {
    timer.start();
    fp = fopen("/sd/log.txt", "w");
    fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
-   myled = 0;
-   loop_measure.attach(&_measure,1.0/RATE);
-   while(1){
-        if(sw==0)_Launch();
+   loop_log.attach(&_Log,1.0/RATE_LOG);
+   loop_open.attach(&_Open,1.0/RATE_OPEN);
+   while(1);
+}
+
+void _Open(){
+   myled = 1 << (Phase-1);
+   switch (Phase) {
+      case STANDBY://スタンバイモード（発射判定不可）
+                  Phase = LAUNCH;
+                  break;
+      case LAUNCH://フライトモード（発射判定可）
+                  alt_launch = ms5607.getAltitude(); 
+                  if(sw==0)Phase = FLIGHT;
+                  break;
+      case FLIGHT://飛翔中（頂点判定可）
+                  alt=ms5607.getAltitude();
+                  if(alt > alt_max) alt_max = alt;
+                  else if(alt_max - alt > 1.5) Phase = OPEN_1;
+                  t_top = timer.read();
+                  break;
+      case OPEN_1://パラシュート開放モード
+                  if(alt_max>1.5) cnt_drop++;
+                  if(cnt_drop==5){
+                     _Servo(1,UNLOCK);
+                     Phase = OPEN_2;
+                     cnt_drop = 0;
+                  }
+      case OPEN_2://缶サット開放モード
+                  if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++;
+                  if (cnt_drop==5) _Servo(2,UNLOCK);
+                  break;
    }
 }
+
+void _Log(){
+   t[row][col] = timer.read();
+   pressure[row][col] = ms5607.getPressure();
+   temperature[row][col] = ms5607.getTemperature();
+   mpu.getAccelero(&acc[row][col][0]);
+   mpu.getGyro(&gyro[row][col][0]);
+   fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
+               t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0],
+               acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
+            );
+   if(col++==RATE_LOG){
+      fclose(fp);
+      fp = fopen("/sd/log.txt", "a");
+      row =! row;
+      col = 0;
+    }
+}
+
+void _Servo(int8_t door_num, bool door_motion){
+   
+}