Diff: main.cpp

Revision:

12:083662bca47d

Parent:

11:d44d137831b9

Child:

13:b088f0db7158

--- a/main.cpp	Sun Feb 10 18:13:37 2019 +0000
+++ b/main.cpp	Tue Feb 12 12:56:21 2019 +0000
@@ -1,23 +1,30 @@
 #include "mbed.h"
 #include "MPU6050_DMP6.h"
 #include "HMC5883L.h"
+#include "SDFileSystem.h"
+#include "SkipperSv2.h"
+#include "falfalla.h"
 
 //MPU_check用
 #define PI 3.14159265358979
 
 #define servo_NEUTRAL_R    1614
 #define servo_NEUTRAL_L    1621
-#define servo_FORWARD_R    1860
-#define servo_FORWARD_L    1860
-#define servo_back_R       1060
-#define servo_back_L       1060
 #define servo_slow_FORWARD_R    1560
 #define servo_slow_FORWARD_L    1560
-#define servo_slow_back_R       1360
-#define servo_slow_back_L       1360
-#define TurnTable_NEUTRAL     1180 //カメラ台座のサーボ
-#define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
-#define Focus_NEUTRAL  1455  //焦点合わせ用サーボの最小値
+#define servo_low_FORWARD_R     1560   //RasPi速さ調節用 
+#define servo_high_FORWARD_R    1860
+#define servo_low_FORWARD_L     1560
+#define servo_high_FORWARD_L    1860
+#define servo_low_back_R        1360
+#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 camera_deg_A 1400   //カメラ角度調整
+#define camera_deg_B 1800
 
 #define MOVE_NEUTRAL    0
 #define MOVE_FORWARD    1   
@@ -30,6 +37,19 @@
 #define MAX_FORWARD     8          //はやい_姿勢修正用
 #define MAX_BACK        9
 
+int Servo_NEUTRAL_R;               //SDcard用宣言
+int Servo_NEUTRAL_L;
+int Servo_high_FORWARD_R;
+int Servo_high_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 Focus_NEUTRAL;//カメラ焦点
+int Camera_deg_A;//カメラ角度調整
+int Camera_deg_B;
 
 void getSF_Serial_jevois();
 void getSF_Serial_pi();
@@ -47,6 +67,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_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,
+            int *Camera_deg_A, int *Camera_deg_B
+            );
+
 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
 
@@ -60,7 +90,7 @@
 
 RawSerial pc(PA_2,PA_3,115200); //uart2
 //pa2:UART2_TX,pa3:UART2_RX
-//RawSerial pc2(PB_6,PB_7,115200); //uart1
+RawSerial pc2(PA_11,PA_12,115200); //uart1
 //pb6:UART1_TX,pb7:UART1_RX
 
 //PWM pin宣言
@@ -71,7 +101,6 @@
 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
 PwmOut servoGetUP(PC_8);       //skipperウラ 起き上がり動作
-PwmOut servoParachute(PA_11);  //skipper USB端子より パラシュート切り離し
 
 /*通信用のpinは
   PA_3(UART2_Rx)   skipperウラ
@@ -99,7 +128,7 @@
     
     // シリアル通信受信の割り込みイベント登録
     pc.attach(getSF_Serial_jevois, Serial::RxIrq);
-    //pc2.attach(getSF_Serial_pi, Serial::RxIrq);
+    pc2.attach(getSF_Serial_pi, Serial::RxIrq);
     
     while(1) {
         if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
@@ -134,28 +163,30 @@
             break;
             
         case 'Y': //MOVE_FORWARD
-            servoR.pulsewidth_us(servo_FORWARD_R);
-            servoL.pulsewidth_us(servo_FORWARD_L);
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
-        case 'L': //MOVE_LEFT 
-            servoR.pulsewidth_us(servo_slow_FORWARD_R);
-            servoL.pulsewidth_us(servo_slow_back_L);
+        case 'l': //MOVE_LEFT Low Speed
+            servoR.pulsewidth_us(servo_low_FORWARD_R);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            //NVIC_EnableIRQ(USART2_IRQn); 
+        
+        case 'L': //MOVE_LEFT High Speed
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             
-        case 'R': //MOVE_RIGHT  
-            servoR.pulsewidth_us(servo_slow_back_R);
-            servoL.pulsewidth_us(servo_slow_FORWARD_L);
+        case 'r': //MOVE_RIGHT Low Speed  
+            servoL.pulsewidth_us(servo_low_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
-            
-        case 'B': //MOVE_BACK 
-            servoR.pulsewidth_us(servo_back_R);
-            servoL.pulsewidth_us(servo_back_L);
+        
+        case 'R': //MOVE_RIGHT  High Speed
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
@@ -166,10 +197,50 @@
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
+        
+        case '1': //MOVE_FORWARD Speed Level 1
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            wait(1);
+            do{
+            }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
+        
+        case '2': //MOVE_FORWARD Speed Level 2
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            wait(2);
+            do{
+            }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
+        
+        case '3': //MOVE_FORWARD Speed Level 3
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            wait(3);
+            do{
+            }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
+        
+        case '4': //MOVE_FORWARD Speed Level 4
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            wait(4);
+            do{
+            }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
             
-        case 'J': //MOVE_FORWARD Tim
-            servoR.pulsewidth_us(servo_FORWARD_R);
-            servoL.pulsewidth_us(servo_FORWARD_L);
+        case '5': //MOVE_FORWARD Speed Level 5
+            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoL.pulsewidth_us(servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             wait(5);
             do{
@@ -178,15 +249,19 @@
             break;
             
         case 'M': //MatchPosition
-            servoR.pulsewidth_us(MatchSpeed);
+            servoR.pulsewidth_us(matchspeed);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
         
-        case 'T': //jevoisからraspberry piへの切り替え
+        case 'P': //jevoisからraspberry piへの切り替え
             jevoisFlag = false;
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
+        
+        case 'S':
+            /*RasPiからの超音波判定(プログラムスタート部)*/
+            break;
             
         default :
             //NVIC_EnableIRQ(USART1_IRQn);
@@ -203,7 +278,7 @@
     g_landingcommand='N';
     servoTurnTable.pulsewidth_us(1800);
     wait_ms(600);
-    servoTurnTable.pulsewidth_us(TurnTable_NEUTRAL);
+    servoTurnTable.pulsewidth_us(turntable_NEUTRAL);
     if(jevoisFlag == true) FocusAdjust();
     else wait(1);
     
@@ -295,7 +370,7 @@
 }
 
 
-/*void getSF_Serial_pi(){
+void getSF_Serial_pi(){
     
     //NVIC_DisableIRQ(USART2_IRQn);
 
@@ -342,11 +417,18 @@
     
     //NVIC_EnableIRQ(USART2_IRQn);
                     
-}*/
+}
 
 
 void setup(){
      
+    /*SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
+               &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
+               &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
+               &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
+               &Camera_deg_A, &Camera_deg_B
+               );*/
+     
     Init_sensors();
     //switch2.rise(ResetTrim);
     
@@ -501,6 +583,138 @@
     flg_checkoutlier = false;
     
 }
+
+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,
+               int *Camera_deg_A, int *Camera_deg_B
+               ){
+
+    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",
+        "CAMERA_DEG_A",
+        "CAMERA_DEG_B"
+    };
+
+    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++;
+        }
+         if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
+        else{                                         *Camera_deg_A = camera_deg_A;
+                                                      SDerrorcount++;
+        }
+         if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
+        else{                                         *Camera_deg_B = camera_deg_B;
+                                                      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;
+        *Camera_deg_A = camera_deg_A;
+        *Camera_deg_B = camera_deg_B;
+        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 = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
+    pc.printf("Servo_high_FORWARD_R = %d, Servo_high_FORWARD_L = %d, kdRUD = %f\r\n", *Servo_high_FORWARD_R, *Servo_high_FORWARD_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, Matchspeed =  %f\r\n", *Turntable_NEUTRAL, *Matchspeed);
+    pc.printf("Focus_NEUTRAL = %fr\n", *Focus_NEUTRAL);
+    pc.printf("Camera_deg_A = %f, Camera_deg_B =  %f\r\n", *Camera_deg_A, *Camera_deg_B);
+    
+    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地磁気センサ_デバック用