航空研究会
sdカード完成
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
Diff: main.cpp
- Revision:
- 13:d8f49328c57f
- Parent:
- 12:a2c6207cbce3
--- a/main.cpp Tue Feb 12 02:59:22 2019 +0000
+++ b/main.cpp Tue Feb 12 06:28:06 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
@@ -17,9 +18,9 @@
#define servo_high_back_R 1160
#define servo_low_back_L 1360
#define servo_high_back_L 1160
-#define TurnTable_NEUTRAL 1180 //カメラ台座のサーボ
-#define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
-#define Focus_NEUTRAL 1455 //焦点合わせ用サーボ
+#define turntable_NEUTRAL 1180 //カメラ台座のサーボ
+#define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
+#define focus_NEUTRAL 1455 //焦点合わせ用サーボ
#define MOVE_NEUTRAL 0
#define MOVE_FORWARD 1
@@ -41,8 +42,8 @@
int Servo_slow_FORWARD_R;
int Servo_slow_FORWARD_L;
int Turntable_NEUTRAL;
-int Matchspeed;
-int Minfocus;
+int Matchspeed; //機体角度調整
+int Focus_NEUTRAL;//カメラ焦点
void getSF_Serial_jevois();
void getSF_Serial_pi();
@@ -63,11 +64,10 @@
//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 SetOptions(int Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
+ int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
- int *Turntable_NEUTRAL,int *Matchspeed,int *Minfocus
+ int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL
);
static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
@@ -243,7 +243,7 @@
break;
case 'M': //MatchPosition
- servoR.pulsewidth_us(MatchSpeed);
+ servoR.pulsewidth_us(matchspeed);
//NVIC_EnableIRQ(USART1_IRQn);
//NVIC_EnableIRQ(USART2_IRQn);
break;
@@ -272,7 +272,7 @@
g_landingcommand='N';
servoTurnTable.pulsewidth_us(1800);
wait_ms(600);
- servoTurnTable.pulsewidth_us(TurnTable_NEUTRAL);
+ servoTurnTable.pulsewidth_us(Servo_NEUTRAL_R);
if(jevoisFlag == true) FocusAdjust();
else wait(1);
@@ -304,14 +304,14 @@
}
void FocusAdjust(){
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
+ servoCameraPinto.pulsewidth_us(focus_NEUTRAL-200);
pc.printf("set\r\n");
wait(1);
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL+30);
+ servoCameraPinto.pulsewidth_us(focus_NEUTRAL+30);
pc.printf("check\r\n");
wait(3);
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
+ servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
return;
}
@@ -571,6 +571,121 @@
}
+int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
+ int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
+ int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
+ int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL
+ ){
+
+ pc.printf("SDsetup start.\r\n");
+
+ FILE *fp;
+ char parameter[20]; //文字列渡す用の配列
+ int SDerrorcount = 0; //取得できなかった数を返す
+ const char *paramNames[] = {
+ "SERVO_NEUTRAL_R",
+ "SERVO_NEUTRAL_L",
+ "SERVO_HIGH_FORWARD_R",
+ "SERVO_HIGH_FORWARD_L",
+ "SERVO_SLOW_FORWARD_R",
+ "SERVO_SLOW_FORWARD_L",
+ "TURNTABLE_NEUTRAL",
+ "MATCH_SPEED",
+ "FOCUS_NEUTRAL"
+ };
+
+ 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_high_FORWARD_R = atof(parameter);
+ else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
+ else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
+ else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
+ else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
+ SDerrorcount++;
+ }
+
+ if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
+ else{ *Turntable_NEUTRAL = turntable_NEUTRAL;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
+ else{ *Matchspeed = matchspeed;
+ SDerrorcount++;
+ }
+ if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
+ else{ *Focus_NEUTRAL = focus_NEUTRAL;
+ SDerrorcount++;
+ }
+
+ fclose(fp);
+
+ }else{ //ファイルがなかったら
+ pc.printf("fp was null.\r\n");
+
+ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
+ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
+ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
+ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
+ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
+ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
+ *Turntable_NEUTRAL = turntable_NEUTRAL;
+ *Matchspeed = matchspeed;
+ *Focus_NEUTRAL = focus_NEUTRAL;
+ 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(){