Diff: main.cpp

Revision:

11:8427ecccf07d

Parent:

10:63fe920595a7

Child:

12:08ba6996246b

diff -r 63fe920595a7 -r 8427ecccf07d main.cpp
--- a/main.cpp	Sun Feb 10 09:05:27 2019 +0000
+++ b/main.cpp	Mon Feb 11 10:58:57 2019 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "MPU6050_DMP6.h"
 #include "HMC5883L.h"
+#include "SDFileSystem.h"
 
 //MPU_check用
 #define PI 3.14159265358979
@@ -47,6 +48,16 @@
 void MatchPosition();
 void FocusAdjust();
 
+//sd設定
+int GetParameter(FILE *fp, const char *paramName,char parameter[]);
+
+int SetOptions(int *servo_NEUTRAL_R, int *servo_NEUTRAL_L,
+               int *servo_FORWARD_R, int *servo_FORWARD_L,
+               int *servo_back_R, int *servo_back_L,
+               int *servo_slow_FORWARD_R,int *servo_slow_FORWARD_L,
+               int *TurnTable_NEUTRAL,int *MatchSpeed,int *minFocus
+               );
+
 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
 
@@ -84,6 +95,8 @@
 RawSerial pc2(PB_6,PB_7,115200); //uart1
 //pb6:UART1_TX,pb7:UART1_RX
 
+SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");//sdカード
+
 char g_landingcommand='N';
 
 //MPU_check用
@@ -343,6 +356,10 @@
 
 void setup(){
      
+    SetOptions(&servo_NEUTRAL_R, &servo_NEUTRAL_L,&servo_FORWARD_R,&servo_FORWARD_L,&servo_back_R,&servo_back_L,&servo_slow_FORWARD_R,
+               &servo_slow_FORWARD_L,&TurnTable_NEUTRAL,&MatchSpeed,&minFocus
+               );    
+     
     Init_sensors();
     //switch2.rise(ResetTrim);
     
@@ -497,6 +514,136 @@
     flg_checkoutlier = false;
     
 }
+               
+/sdによる設定
+int SetOptions(int *servo_NEUTRAL_R, int *servo_NEUTRAL_L,
+               int *servo_FORWARD_R, int *servo_FORWARD_L,
+               int *servo_back_R, int *servo_back_L,
+               int *servo_slow_FORWARD_R,int *servo_slow_FORWARD_L,
+               int *TurnTable_NEUTRAL,int *MatchSpeed,int *minFocus
+               ){
+
+    pc.printf("SDsetup start.\r\n");    
+    
+    FILE *fp;
+    char parameter[20]; //文字列渡す用の配列
+    int SDerrorcount = 0;  //取得できなかった数を返す
+    const char *paramNames[] = { 
+        "SERVO_NEUTRAL_R",
+        "SERVO_NEUTRAL_L",
+        "SERVO_FORWARD_R",
+        "SERVO_FORWARD_L",
+        "SERVO_BACK_R",
+        "SERVO_BACK_L",
+        "SERVO_SLOW_FORWARD_R",
+        "SERVO_SLOW_FORWARD_L",
+        "TURNTABLE_NEUTRAL",
+        "MATCH_SPEED",
+        "MINFOCUS"
+    };
+
+    fp = fopen("/sd/option.txt","r");
+    
+    if(fp != NULL){ //開けたら
+        pc.printf("File was openned.\r\n");
+        if(GetParameter(fp,paramNames[0],parameter)) *servo_NEUTRAL_R = atof(parameter);
+        else{                                        *servo_NEUTRAL_R = SERVO_NEUTRAL_R;
+                                                     SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[1],parameter)) *servo_NEUTRAL_L = atof(parameter);
+        else{                                        *servo_NEUTRAL_L = SERVO_NEUTRAL_L;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[2],parameter)) *servo_FORWARD_R = atof(parameter);
+        else{                                        *servo_FORWARD_R = SERVO_FORWARD_R;
+                                                     SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[3],parameter)) *servo_FORWARD_L = atof(parameter);
+        else{                                        *servo_FORWARD_L = SERVO_FORWARD_L;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[4],parameter)) *servo_back_R = atof(parameter);
+        else{                                        *servo_back_R = SERVO_BACK_R;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[5],parameter)) *servo_back_L = atof(parameter);
+        else{                                        *servo_back_L = SERVO_BACK_L;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[6],parameter)) *servo_slow_FORWARD_R = atof(parameter);
+        else{                                        *servo_slow_FORWARD_R = SERVO_SLOW_FORWARD_R;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[7],parameter)) *servo_slow_FORWARD_L = atof(parameter);
+        else{                                        *servo_slow_FORWARD_L = SERVO_SLOW_FORWARD_L
+                                                      SDerrorcount++;
+        }
+        
+        if(GetParameter(fp,paramNames[10],parameter)) *TurnTable_NEUTRAL = atof(parameter);
+        else{                                         *TurnTable_NEUTRAL = TURNTABLE_NEUTRAL;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[11],parameter)) *MatchSpeed = atof(parameter);
+        else{                                         *MatchSpeed = MATCH_SPEED;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[12],parameter)) *minFocus = atof(parameter);
+        else{                                         *minFocus = MINFOCUS;
+                                                      SDerrorcount++;
+        }
+        
+        fclose(fp);
+
+    }else{  //ファイルがなかったら
+        pc.printf("fp was null.\r\n");
+        
+        *servo_NEUTRAL_R = SERVO_NEUTRAL_R;
+        *servo_NEUTRAL_L = SERVO_NEUTRAL_L;
+        *servo_FORWARD_R = SERVO_FORWARD_R;
+        *servo_FORWARD_L = SERVO_FORWARD_L;
+        *servo_back_R = SERVO_BACK_R;
+        *servo_back_L = SERVO_BACK_L;
+        *servo_slow_FORWARD_R = SERVO_SLOW_FORWARD_R;
+        *servo_slow_FORWARD_L = SERVO_SLOW_FORWARD_L;
+        *TurnTable_NEUTRAL = TURNTABLE_NEUTRAL;
+        *MatchSpeed = MATCH_SPEED;
+        *minFocus = MINFOCUS;
+        SDerrorcount = -1;
+    }
+    pc.printf("SDsetup finished.\r\n");
+    if(SDerrorcount == 0)  pc.printf("setting option is success\r\n");
+    else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
+    else if(SDerrorcount > 0)  pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 
+    
+    pc.printf("SERVO_NEUTRAL_R = %f, SERVO_NEUTRAL_L = %f\r\n", *g_kpRUD, *g_kiRUD);
+    pc.printf("SERVO_FORWARD_R = %f, SERVO_FORWARD_L = %f, kdRUD = %f\r\n", *servo_FORWARD_R, *servo_FORWARD_L);
+    pc.printf("SERVO_BACK_R = %f, SERVO_BACK_L = %f\r\n", *servo_back_R, *servo_back_L);
+    pc.printf("SERVO_SLOW_FORWARD_R = %f, SERVO_SLOW_FORWARD_L =  %f\r\n", *servo_slow_FORWARD_R, *servo_slow_FORWARD_L);
+    pc.printf("TURNTABLE_NEUTRAL = %f, MATCH_SPEED = %f\, MINFOCUS = %fr\n", *TurnTable_NEUTRAL, *MatchSpeed,*minFocus);
+    
+    return SDerrorcount;
+}            
+
+int GetParameter(FILE *fp, const char *paramName,char parameter[]){
+    int i=0, j=0;
+    int strmax = 200;
+    char str[strmax];
+
+    rewind(fp); //ファイル位置を先頭に
+    while(1){
+        if (fgets(str, strmax, fp) == NULL) {
+            return 0;
+        }
+        if (!strncmp(str, paramName, strlen(paramName))) {
+            while (str[i++] != '=') {}
+            while (str[i] != '\n') {
+                    parameter[j++] = str[i++];
+            }
+            parameter[j] = '\0';
+            return 1;
+        }
+    }
+}   
     
 void DebugPrint(){
     //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用