Diff: main.cpp

Revision:

19:949bab1e9451

Parent:

18:4566d1f5fc60

Child:

20:011977a37394

--- a/main.cpp	Fri Feb 15 05:13:20 2019 +0000
+++ b/main.cpp	Fri Feb 15 11:28:50 2019 +0000
@@ -22,9 +22,11 @@
 #define pint_speed              25
 #define pint_wait               3.5
 #define turntable_speed         1800 
+#define calib_x                 110
+#define calib_y                 300
 
 #define MOVE_NEUTRAL    0
-#define MOVE_FORWARD    1   
+#define MOVE_FORWARD    1    
 #define MOVE_LEFT       2
 #define MOVE_RIGHT      3
 #define MOVE_BACK       4
@@ -59,7 +61,8 @@
             int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
             int *Camera_deg_A, int *Camera_deg_B,
             int *Pint_speed, float *Pint_wait,
-            int *Turntable_speed
+            int *Turntable_speed,
+            int *Calib_x, int *Calib_y
             );
 
 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
@@ -116,6 +119,16 @@
     
     //MPU_check
     setup();
+    //コンパスチェック用
+    /*while(1){
+        SensingMPU();
+        SensingHMC();
+        pc.printf("%3.2f\t",nowAngle[2]);
+        pc.printf("%3.2f\t",nowAngle_HMC); //HMC
+        pc.printf("\r\n");
+        wait_ms(30);
+    }*/
+        
     
     servoCameradeg.pulsewidth_us(Camera_deg_A);
     
@@ -427,7 +440,8 @@
                &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
                &Camera_deg_A, &Camera_deg_B,
                &Pint_speed,&Pint_wait,
-               &Turntable_speed
+               &Turntable_speed,
+               &Calib_x, &Calib_y
                );
      
     Init_sensors();
@@ -548,17 +562,17 @@
     static bool flg_checkoutlier = false;
     NVIC_DisableIRQ(USART1_IRQn);
     NVIC_DisableIRQ(USART2_IRQn);
-    NVIC_DisableIRQ(TIM5_IRQn);
-    NVIC_DisableIRQ(EXTI0_IRQn);
-    NVIC_DisableIRQ(EXTI9_5_IRQn);
+    //NVIC_DisableIRQ(TIM5_IRQn);
+    //NVIC_DisableIRQ(EXTI0_IRQn);
+    //NVIC_DisableIRQ(EXTI9_5_IRQn);
 
-    rpy= compass.getHeadingXYDeg(20,50);
+    rpy= compass.getHeadingXYDeg(Calib_x,Calib_y);
  
     NVIC_EnableIRQ(USART1_IRQn);
     NVIC_EnableIRQ(USART2_IRQn);
-    NVIC_EnableIRQ(TIM5_IRQn);
-    NVIC_EnableIRQ(EXTI0_IRQn);
-    NVIC_EnableIRQ(EXTI9_5_IRQn);
+    //NVIC_EnableIRQ(TIM5_IRQn);
+    //NVIC_EnableIRQ(EXTI0_IRQn);
+    //NVIC_EnableIRQ(EXTI9_5_IRQn);
     
     
     //外れ値対策
@@ -591,7 +605,8 @@
                int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
                int *Camera_deg_A, int *Camera_deg_B,
                int *Pint_speed, float *Pint_wait,
-               int *Turntable_speed
+               int *Turntable_speed,
+               int *Calib_x, int *Calib_y
                ){
 
     pc.printf("SDsetup start.\r\n");    
@@ -613,7 +628,9 @@
         "CAMERA_DEG_B",
         "PINT_SPEED",
         "PINT_WAIT",
-        "TURNTABLE_SPEED"
+        "TURNTABLE_SPEED",
+        "CALIB_X",
+        "CALIB_Y"
     };
 
     fp = fopen("/sd/option.txt","r");
@@ -657,26 +674,34 @@
         else{                                         *Focus_NEUTRAL = focus_NEUTRAL;
                                                       SDerrorcount++;
         }
-         if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
+        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);
+        if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
         else{                                         *Camera_deg_B = camera_deg_B;
                                                       SDerrorcount++;
         }
-         if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
+        if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
         else{                                         *Pint_speed = pint_speed;
                                                       SDerrorcount++;
         }
-         if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
-        else{                                         *Pint_wait = pint_wait;
+        if(GetParameter(fp,paramNames[12],parameter)) *Turntable_speed = atof(parameter);
+        else{                                         *Turntable_speed = turntable_speed;
                                                       SDerrorcount++;
         }
-         if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
+        if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
         else{                                         *Turntable_speed = turntable_speed;
                                                       SDerrorcount++;
         }
+        if(GetParameter(fp,paramNames[14],parameter)) *Calib_x = atof(parameter);
+        else{                                         *Calib_x = calib_x;
+                                                      SDerrorcount++;
+        }
+        if(GetParameter(fp,paramNames[15],parameter)) *Calib_y = atof(parameter);
+        else{                                         *Calib_y = calib_y;
+                                                      SDerrorcount++;
+        }
         fclose(fp);
 
     }else{  //ファイルがなかったら
@@ -696,6 +721,8 @@
         *Pint_speed = pint_speed;
         *Pint_wait = pint_wait;
         *Turntable_speed = turntable_speed;
+        *Calib_x = calib_x;
+        *Calib_y = calib_y;
         SDerrorcount = -1;
     }
     pc.printf("SDsetup finished.\r\n");
@@ -711,10 +738,10 @@
     pc.printf("Camera_deg_A = %d, Camera_deg_B =  %d\r\n", *Camera_deg_A, *Camera_deg_B);
     pc.printf("Pint_speed = %d, Pint_wait =  %f\r\n", *Pint_speed, *Pint_wait);
     pc.printf("Turntable_speed = %d\r\n",*Turntable_speed);
+    pc.printf("Calib_x = %d, Calib_y =  %d\r\n", *Calib_x, *Calib_y);
     
     return SDerrorcount;
 }            
-
 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
     int i=0, j=0;
     int strmax = 200;