Diff: main.cpp

Branch:

mpu????????

Revision:

67:94fe2180f39a

Parent:

66:2d1a7f270e27

Child:

68:ae3c3500074c

--- a/main.cpp	Fri Jun 30 22:00:34 2017 +0000
+++ b/main.cpp	Sat Jul 15 02:45:03 2017 +0000
@@ -17,16 +17,22 @@
 #define MPU_LOOP_TIME 0.01
 #define MPU_DELT_MIN 250
 
-#define ERURON_MOVE_DEG_INI_R 14.4//18.0       //degree
-#define DRUG_MOVE_DEG_INI_R -0.49*0.8
+#define ERURON_MOVE_DEG_INI_R 14.4*0.8//18.0       //degree
+#define DRUG_MOVE_DEG_INI_R 0.49
 #define ERURON_TRIM_INI_R 0.39633  //値lowerすると頭上げ 0.37で後縁一致
 #define DRUG_TRIM_INI_R  0.37
 
 #define ERURON_MOVE_DEG_INI_L -15.52//-19.4     //degree
 #define DRUG_MOVE_DEG_INI_L -0.52
-#define ERURON_TRIM_INI_L  0.49067  // 値を大きくすると頭上げ
+#define ERURON_TRIM_INI_L  0.41567  // 値を大きくすると頭上げ
 #define DRUG_TRIM_INI_L    0.73//値を小さくすると開く側
 
+
+#define debug pc.printf
+#define debugMPU //pc.printf
+
+const char *configfilename = "/local/CONFIG.csv";
+
 /*ドラッグラダー
 初期値 0.65
 最大角0.99
@@ -55,6 +61,8 @@
 MPU6050 mpu6050;
 Timer t;
 
+LocalFileSystem local("local");
+
 //Set up I2C, (SDA,SCL)
 I2C i2c_mpu(p9,p10);
 
@@ -168,7 +176,7 @@
         gyro_c[3]=(char)(int)((roll - (int)roll)*100);
         gyro_c[4]=(char)((int)yaw);
         gyro_c[5]=(char)(int)((yaw - (int)yaw)*100);
-        pc.printf("p:%f,r:%f,y:%f\n\r",pitch,roll,yaw);
+        debugMPU("p:%f,r:%f,y:%f\n\r",pitch,roll,yaw);
         Thread::wait(1);
     }//while(1)
 }
@@ -207,19 +215,60 @@
     double servoDeg = ConvertDeg(erebonDeg);
     return GetValueByHorn(servoDeg);
 }
+// configファイル作成
+int makeConfigFile() {
+    FILE *fp;
+    if((fp = fopen(configfilename, "w")) == NULL) {
+        pc.printf("can't open %s\n", configfilename);
+        return -1;
+    }
+    fprintf(fp, "// This is Yokutan %s.\n", IsRPin ? "R" : "L");
+    fprintf(fp, "eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg\n");
+    fclose(fp);
+    return 0;
+}
+
+// config内容をファイルに追記
+int writeConfig() {
+    FILE *fp;
+    if((fp = fopen(configfilename, "a")) == NULL) {
+        pc.printf("can't open %s\n", configfilename);
+        return -1;
+    }
+    fprintf(fp, "%f,%f,%f,%f\n", eruronTrim, drugTrim, eruronMoveDeg, drugMoveDeg);
+    fclose(fp);
+    return 0;
+}
+// configの最新情報を読み込み
+int readConfig() {
+    FILE *fp;
+    char s[256];
+    if((fp = fopen(configfilename, "r")) == NULL) {
+        return -1;
+    }
+    while(fgets(s, 255, fp) != NULL) {
+        sscanf(s, "%f,%f,%f,%f\n", &eruronTrim, &drugTrim, &eruronMoveDeg, &drugMoveDeg);
+    }
+    fclose(fp);
+    debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
+    return 0;
+}
 
 void init()
 {
-    if(IsRPin) {
-        eruronTrim = ERURON_TRIM_INI_R;
-        drugTrim = DRUG_TRIM_INI_R;
-        eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
-        drugMoveDeg =DRUG_MOVE_DEG_INI_R;
-    } else {
-        eruronTrim = ERURON_TRIM_INI_L;
-        drugTrim = DRUG_TRIM_INI_L;
-        eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
-        drugMoveDeg = DRUG_MOVE_DEG_INI_L;
+    if(readConfig() == -1) {
+        makeConfigFile();
+        if(IsRPin) {
+            eruronTrim = ERURON_TRIM_INI_R;
+            drugTrim = DRUG_TRIM_INI_R;
+            eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
+            drugMoveDeg =DRUG_MOVE_DEG_INI_R;
+        } else {
+            eruronTrim = ERURON_TRIM_INI_L;
+            drugTrim = DRUG_TRIM_INI_L;
+            eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
+            drugMoveDeg = DRUG_MOVE_DEG_INI_L;
+        }
     }
     SERVO_FLAG = servoInit();
     INA_FLAG = inaInit();
@@ -317,11 +366,17 @@
 //    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
 
     while(1) {
-        while(setTrimPin) {
-            setTrim();
+        if(setTrimPin) {
+            do {
+                setTrim();
+            } while(setTrimPin);
+            writeConfig();
         }
-        while (setMaxDegPin) {
-            setMaxDeg();
+        if(setMaxDegPin) {
+            do {
+                setMaxDeg();
+            } while(setMaxDegPin);
+            writeConfig();
         }
         led4 = 0;
         led2 = 0;