usb実装中
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
Diff: main.cpp
- Revision:
- 11:8427ecccf07d
- Parent:
- 10:63fe920595a7
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地磁気センサ_デバック用