Revision 13:d8f49328c57f, committed 2019-02-12

Comitter:

taknokolat

Date:

Tue Feb 12 06:28:06 2019 +0000

Parent:

12:a2c6207cbce3

Commit message:

a

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Tue Feb 12 06:28:06 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/1519026547/code/SDFileSystem3/#85fbe134acff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config/SkipperSv2.h	Tue Feb 12 06:28:06 2019 +0000
@@ -0,0 +1,22 @@
+#ifndef SKIPPERSV2_H_
+#define SKIPPERSV2_H_
+
+/*フライトモードの変数================
+    ModeBreak       モードブレイク         状態変更時に１回だけ呼び出して、現在実行中のRTOSタスクを全部無効にする
+    StartUP         初期化モード          まず最初に呼び出して、各種設定を有効にし、機体のチェック等を行う
+    ManualMode      手動飛行モード         通常のラジコンと同じ状態
+    AutoLoop        自動操縦　水平旋回モード
+    AutoMobius      自動操縦　８の字飛行モード
+    AutoClimb       自動操縦　上昇旋回
+    AutoGlide       自動操縦　自動滑空
+    AutoLanding     自動操縦　自動着陸
+*/
+    
+    #define MAGBIASX 0   //-63
+    #define MAGBIASY 0   //580
+    #define MAGBIASZ 0   //132
+
+bool Flag_ChangeMode=true;      //モードを切り替えた瞬間を検出してtrueを返す　モード変更が直ちに行われ、falseになる。
+//=========== end =============
+
+#endif /* SKIPPERSV2_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config/falfalla.h	Tue Feb 12 06:28:06 2019 +0000
@@ -0,0 +1,68 @@
+#ifndef FALFALLA_H
+#define FALFALLA_H
+
+#define SWITCH_CHECK 1400
+#define NUMBER_FALFALLA 2 //何号機かを入れるとそれに応じて#if以下が変わって設定が変わる
+
+static int servo_NEUTRAL_R = 1614;
+static int servo_NEUTRAL_L = 1621;
+static int servo_high_FORWARD_R = 1860;
+static int servo_high_FORWARD_L = 1860
+static int servo_slow_FORWARD_R = 1560;
+static int servo_slow_FORWARD_L = 1560;
+static int turntable_NEUTRAL = 1500;//カメラ台座のサーボ
+static int matchspeed = servo_NEUTRAL_R + 100; //カメラと方向を合わせるときの車輪の速度
+static int minfocus = 1200;  //焦点合わせ用サーボの最小値
+
+static float g_rightloopROLL=-17.0;
+static float g_rightloopROLL_approach=-17.0;
+static float g_rightloopPITCH=-6.3;
+static float g_rightloopPITCH_approach = -15.0;
+static float g_leftloopROLL=16.0;
+static float g_leftloopROLL_approach=16.0;
+static float g_leftloopPITCH=-6.0;
+static float g_leftloopPITCH_approach=-13.0;
+static float g_gostraightROLL=2.0;
+static float g_gostraightPITCH=-3.0;
+static float g_takeoffTHR=1.0;
+static float g_loopTHR=0.58;
+
+static float g_rightloopROLLshort=-20.0;
+static float g_rightloopPITCHshort=-7.0;
+static float g_leftloopROLLshort=22.0;
+static float g_leftloopPITCHshort=-6.0;
+
+static float g_glideloopROLL = -5.0;      //rewrite
+static float g_glideloopPITCH = 0.0;
+static float g_autoonPITCH = -3.0;
+
+static int g_rightloopRUD = 1500;
+static int g_rightloopRUD_approach=1500;
+static int g_rightloopshortRUD = 1500;
+static int g_leftloopRUD = 1500;
+static int g_leftloopRUD_approach = 1710;
+static int g_leftloopshortRUD = 1500;
+static int g_glideloopRUD = 1500;
+
+static int g_AIL_L_correctionrightloop = 0;
+static int g_AIL_L_correctionrightloopshort = 0;
+static int g_AIL_L_correctionleftloop = 0;
+static int g_AIL_L_correctionleftloopshort = 0;
+
+
+#if NUMBER_FALFALLA == 1    //1号機
+const int8_t Reverce_falfalla[4] = {1,-1,1,1};   //Nutral:1 , Reverce:-1
+
+#elif NUMBER_FALFALLA == 2  //2号機
+const int16_t Reverce_falfalla[4] = {1,-1,1,-1};   //Nutral:1 , Reverce:-1
+
+#endif
+
+
+#endif /* ESTRELA_H_ */
+
+/*
+x軸回り    ロール
+y軸回り    ピッチ
+z軸回り    ヨー
+*/
\ No newline at end of file

--- 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(){