Revision 12:211d2c6d2afc, committed 2017-03-03

Comitter:

mikawataru

Date:

Fri Mar 03 01:26:07 2017 +0000

Parent:

11:b61d0fcc2ed3

Child:

13:f73b4beded57

Commit message:

add servo control by serial device

Changed in this revision

--- a/main.cpp	Thu Mar 02 22:59:12 2017 +0000
+++ b/main.cpp	Fri Mar 03 01:26:07 2017 +0000
@@ -46,7 +46,7 @@
 /*判定パラメータ*/
 #define ACC_LAUNCH 4//発射判定加速度[g]
 #define DROP_TOP 1.5//頂点判定最低降下量[m]
-#define ALT_CAN 350//缶サット開放高度[m]
+#define ALT_CAN 150//缶サット開放高度[m]
 #define TIME_TOP 15//タイマー開放時間[s]
 /*動作レート*/
 #define RATE_LOG 10//ログ用
@@ -54,12 +54,18 @@
 /*サーボ動作*/
 #define LOCK 0.0005
 #define UNLOCK 0.0015
-/*フェイズ定義*/
+/*フェーズ定義*/
 #define STANDBY 0
 #define LAUNCH 1
 #define FLIGHT 2
 #define DROP 3
-
+/*入力モード定義*/
+#define BAN 0
+#define NORMAL 1
+#define SERVO_1 2
+#define SERVO_2 3
+#define MODE 4
+/**/
 #define P0 1013.25f//海面気圧[hPa]
 #define ACC 4096.0f//加速度オフセット値
 /*ピン指定*/
@@ -92,10 +98,14 @@
 float t_drop,t_top,t_launch;
 int8_t cnt_judge = 0;
 int8_t col_open = 0;
+/*入力用*/
+int8_t Input_mode = NORMAL;
+int8_t input_buff[3] = {};
 /*関数*/
 void _Open();
 void _Servo(int8_t door_num, float motion);
 void _Log();
+void _Input();
 float _GetAlt(float press, float temp);
 float _Median(float data[], int num);
 
@@ -111,6 +121,7 @@
    _Servo(0,LOCK);
    fp = fopen("/sd/log.txt", "w");
    fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
+   device.attach(&_Input,Serial::RxIrq);
    loop_log.attach(&_Log,1.0/RATE_LOG);
    loop_open.attach(&_Open,1.0/RATE_OPEN);
    while(1);
@@ -120,7 +131,7 @@
    myled = 1 << (Phase-1);
    switch (Phase) {
       case STANDBY://スタンバイモード（発射判定不可）
-                  Phase = LAUNCH;
+                  // Phase = LAUNCH;
                   break;
       case LAUNCH://点火モード（発射判定可）
                   acc4open = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
@@ -203,14 +214,57 @@
       Door_2_1.pulsewidth(motion);
       Door_2_2.pulsewidth(motion);
    }else{
-      device.printf("error\r\n");
+      device.printf("servo error:%d\r\n",door_num);
    }
 }
 
+/*入力用*/
+void _Input(){
+   char c = device.getc();
+   switch (Input_mode) {
+      case NORMAL:
+                  if(c=='s'){
+                     Input_mode = SERVO_1;
+                     device.printf("\r\nInput_mode:SERVO\r\n");
+                     device.printf("\r\n0:All\r\n1:Parachute\r\n2:Cansat\r\n>>");
+                  }else{
+                     device.printf("\r\nthis command is not found:%c\r\n",c);    
+                     device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\n");
+                  }
+                  break;
+      case SERVO_1:
+                  input_buff[0] = (int)(c-'0');
+                  if(c=='0'||c=='1'||c=='2'){
+                     Input_mode = SERVO_2;
+                     device.printf("\r\n0:OPEN\r\n1:CLOSE\r\n>>");                     
+                  }else{                 
+                     device.printf("\r\nthis servo_num is not found:%d\r\n",input_buff[0]);    
+                     device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\n>>");
+                     Input_mode = NORMAL;                     
+                  }
+                  break;
+      case SERVO_2:
+                  if(c=='0')_Servo(input_buff[0],UNLOCK);
+                  else if(c=='1')_Servo(input_buff[0],LOCK);
+                  else{ 
+                     device.printf("\r\nthis motion_num is not found:%c\r\n",c);    
+                  }
+                  device.printf("Input_mode:NORMAL\r\ns:SERVO\r\n");
+                  Input_mode = NORMAL;
+                  break;
+      case MODE:
+                  break;
+      case BAN://reset only
+                  break;
+      default:
+                  break;
+   }
+}
+
+/*その他雑関数*/
 float _GetAlt(float press, float temp){
     return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
 }
-
 float _Median(float data[], int num){
    float median;
    float *sort = (float *)malloc(sizeof(float)*num);
@@ -224,10 +278,8 @@
          }
       }
    }
-
    if(num%2!=0)median = sort[num/2];
    else median = (sort[num/2-1]+sort[num/2])/2.0;
-
    free(sort);
    return median;
 }