Diff: main.cpp

Revision:

24:0ad1725c7849

Parent:

23:29b2722bd753

Child:

25:1f938342d5f9

--- a/main.cpp	Mon Feb 18 06:08:24 2019 +0000
+++ b/main.cpp	Mon Feb 18 08:01:23 2019 +0000
@@ -22,8 +22,8 @@
 #define pint_speed              25
 #define pint_wait               3
 #define turntable_speed         1800 
-#define calib_x                 110
-#define calib_y                 300
+#define time360                 110
+#define match_wid                 5
 #define camera_board_wait       100
 
 #define MOVE_NEUTRAL    0
@@ -63,7 +63,7 @@
             int *Camera_deg_A, int *Camera_deg_B,
             int *Pint_speed, float *Pint_wait,
             int *Turntable_speed,
-            int *Calib_x, int *Calib_y,
+            int *Time360, int *Match_wid,
             int *Camera_board_wait
             );
 
@@ -74,6 +74,8 @@
 bool CameraDegFlag = false;
 bool jevoisFlag = true;
 
+int g_CameraDegCounter = 0;    //カメラの回転数をカウント
+
 enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度
 
 Timer t;
@@ -109,7 +111,7 @@
 
 
 //外付けコンパス
-HMC5883L compass(PB_9, PB_8);    //コンパスセンサー TIM4_CH3とCH4
+//HMC5883L compass(PB_9, PB_8);    //コンパスセンサー TIM4_CH3とCH4
 
 char g_landingcommand='N';
 
@@ -313,8 +315,9 @@
     MoveCansat('N');
     //pc.printf("ok\r\n");
     g_landingcommand='N';
-    servoTurnTable.pulsewidth_us(Turntable_speed);
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
     wait_ms(Camera_board_wait);
+    g_CameraDegCounter++;
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     //pc.printf("zoom1\r\n");
     if(jevoisFlag == true) FocusAdjust();
@@ -347,21 +350,30 @@
 
 void MatchPosition(){
     pc.printf("MatchPosition\r\n");
+    
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
+    wait_ms(Camera_board_wait*g_CameraDegCounter);
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
+    
     int SetLoop=0;
     
     while(SetLoop<30){
     SensingMPU();
     wait_ms(20);
-    SensingHMC();
-    wait_ms(20);
+    //SensingHMC();
+    //wait_ms(20);
     //DebugPrint();
     SetLoop++;
     //pc.printf("nowAngle_HMC=%f\tMPU=%f\r\n",nowAngle_HMC,nowAngle[YAW]);
     
     }
     
-    float HighTargetYaw = nowAngle_HMC+5;
-    float LowTargetYaw = nowAngle_HMC-5;
+    static float TargetDeg = 0;
+    
+    TargetDeg = 360*(float)Camera_board_wait*(float)g_CameraDegCounter/(float)Time360;
+    
+    float HighTargetYaw = TargetDeg + Match_wid;
+    float LowTargetYaw = TargetDeg - Match_wid;
     
     
     if(HighTargetYaw >= 360.0) HighTargetYaw = HighTargetYaw - 360.0;
@@ -370,7 +382,7 @@
     
     pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
     
-    MoveCansat('l');
+    MoveCansat('r');
     
     if(HighTargetYaw-LowTargetYaw<0){
         while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
@@ -387,6 +399,7 @@
             pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
         }
     }
+    g_CameraDegCounter = 0;
     return;
 }
 
@@ -517,7 +530,7 @@
                &Camera_deg_A, &Camera_deg_B,
                &Pint_speed,&Pint_wait,
                &Turntable_speed,
-               &Calib_x, &Calib_y,
+               &Time360, &Match_wid,
                &Camera_board_wait
                );
      
@@ -537,14 +550,14 @@
     t.start();
     
     pc.printf("MPU calibration start\r\n");
-    pc.printf("HMC calibration start\r\n");
+    //pc.printf("HMC calibration start\r\n");
     
     float offsetstart = t.read();
     while(t.read() - offsetstart < 26){
         SensingMPU();
-        SensingHMC();
+        //SensingHMC();
         for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
-        pc.printf("\t%3.2f\t",nowAngle_HMC);
+        //pc.printf("\t%3.2f\t",nowAngle_HMC);
         pc.printf("\r\n");
         led1 = !led1;
         led2 = !led2;
@@ -559,9 +572,9 @@
     FirstYAW = nowAngle[YAW];
     nowAngle[YAW] -= FirstYAW;
     
-    g_FirstYAW_HMC = nowAngle_HMC;
-    nowAngle_HMC -=g_FirstYAW_HMC;
-    if(nowAngle_HMC<0)nowAngle_HMC+=360;
+    //g_FirstYAW_HMC = nowAngle_HMC;
+    //nowAngle_HMC -=g_FirstYAW_HMC;
+    //if(nowAngle_HMC<0)nowAngle_HMC+=360;
     
     led1 = 0;
     led2 = 0;
@@ -639,7 +652,7 @@
     pc.printf("\r\n");
 }
     
-void SensingHMC(){
+/*void SensingHMC(){
     //static int16_t deltaT = 0, t_start = 0;
     //t_start = t.read_us();
     
@@ -652,7 +665,7 @@
     //NVIC_DisableIRQ(EXTI0_IRQn);
     //NVIC_DisableIRQ(EXTI9_5_IRQn);
 
-    rpy= compass.getHeadingXYDeg(Calib_x,Calib_y);
+    rpy= compass.getHeadingXYDeg(Time360,Match_wid);
  
     NVIC_EnableIRQ(USART1_IRQn);
     NVIC_EnableIRQ(USART2_IRQn);
@@ -683,7 +696,7 @@
     }else   count_changeRPY++;
     flg_checkoutlier = false;
     
-}
+}*/
 
 int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
                int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
@@ -692,7 +705,7 @@
                int *Camera_deg_A, int *Camera_deg_B,
                int *Pint_speed, float *Pint_wait,
                int *Turntable_speed,
-               int *Calib_x, int *Calib_y,
+               int *Time360, int *Match_wid,
                int *Camera_board_wait
                ){
 
@@ -716,8 +729,8 @@
         "PINT_SPEED",
         "PINT_WAIT",
         "TURNTABLE_SPEED",
-        "CALIB_X",
-        "CALIB_Y",
+        "TIME360",
+        "MATCH_WID",
         "CAMERA_BOARD_WAIT"
     };
 
@@ -782,12 +795,12 @@
         else{                                         *Turntable_speed = turntable_speed;
                                                       SDerrorcount++;
         }
-        if(GetParameter(fp,paramNames[14],parameter)) *Calib_x = atof(parameter);
-        else{                                         *Calib_x = calib_x;
+        if(GetParameter(fp,paramNames[14],parameter)) *Time360 = atof(parameter);
+        else{                                         *Time360 = time360;
                                                       SDerrorcount++;
         }
-        if(GetParameter(fp,paramNames[15],parameter)) *Calib_y = atof(parameter);
-        else{                                         *Calib_y = calib_y;
+        if(GetParameter(fp,paramNames[15],parameter)) *Match_wid = atof(parameter);
+        else{                                         *Match_wid = match_wid;
                                                       SDerrorcount++;
         }
         if(GetParameter(fp,paramNames[16],parameter)) *Camera_board_wait = atof(parameter);
@@ -813,8 +826,8 @@
         *Pint_speed = pint_speed;
         *Pint_wait = pint_wait;
         *Turntable_speed = turntable_speed;
-        *Calib_x = calib_x;
-        *Calib_y = calib_y;
+        *Time360 = time360;
+        *Match_wid = match_wid;
         *Camera_board_wait = camera_board_wait;
         SDerrorcount = -1;
     }
@@ -831,7 +844,7 @@
     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);
+    pc.printf("Time360 = %d, Match_wid =  %d\r\n", *Time360, *Match_wid);
     pc.printf("Camera_board_wait = %d\r\n", *Camera_board_wait);
     
     return SDerrorcount;