2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Branch:
- mpu????????
- Revision:
- 67:94fe2180f39a
- Parent:
- 66:2d1a7f270e27
- Child:
- 68:ae3c3500074c
diff -r 2d1a7f270e27 -r 94fe2180f39a main.cpp --- 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;