Diff: main.cpp

Revision:

32:ab030832d180

Parent:

31:210cc32d3175

Child:

33:5f77118eacd5

--- a/main.cpp	Wed Feb 27 02:39:44 2019 +0000
+++ b/main.cpp	Thu Feb 28 11:59:51 2019 +0000
@@ -26,6 +26,8 @@
 #define match_wid                 5
 #define camera_board_wait       100
 
+#define ReturnCount             2
+
 #define MOVE_NEUTRAL    0
 #define MOVE_FORWARD    1    
 #define MOVE_LEFT       2
@@ -377,6 +379,27 @@
     g_landingcommand='N';
     servoCameradeg.pulsewidth_us(Camera_deg_B);
     wait_ms(30);
+    if(g_CameraDegCounter==0){
+        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
+        wait_ms(Camera_board_wait*ReturnCount);
+        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
+    
+    
+        for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
+                
+                servoCameradeg.pulsewidth_us(Camera_deg_B-i);
+                servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
+                servoL.pulsewidth_us(Servo_NEUTRAL_L);
+                wait_ms(30); 
+                //pc.printf("zoom2\r\n");  
+                if(jevoisFlag == true) FocusAdjust();
+                else wait(1);
+                if(g_landingcommand!='N') return;  
+        
+        }
+        
+    }
+    
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
     wait_ms(Camera_board_wait);
     g_CameraDegCounter++;
@@ -384,12 +407,16 @@
     //pc.printf("zoom1\r\n");
     if(jevoisFlag == true) FocusAdjust();
     else wait(1);
+        
+    
     if(g_landingcommand!='N') return;
     
     for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
             
-             servoCameradeg.pulsewidth_us(Camera_deg_B-i);
-             wait_ms(30); 
+            servoCameradeg.pulsewidth_us(Camera_deg_B-i);
+            servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
+            servoL.pulsewidth_us(Servo_NEUTRAL_L);
+            wait_ms(30); 
             //pc.printf("zoom2\r\n");  
             if(jevoisFlag == true) FocusAdjust();
             else wait(1);
@@ -398,15 +425,27 @@
     }
     
     
+    pc.printf("g_CameraDegCounter = %d\r\n",g_CameraDegCounter);
     //pc.printf("Move Board Finish\r\n");  
     return;
 }
 
 void MatchPosition(){
+    NVIC_DisableIRQ(USART6_IRQn);
+    NVIC_DisableIRQ(USART2_IRQn);
     pc.printf("MatchPosition\r\n");
     
-    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
-    wait_ms(Camera_board_wait*g_CameraDegCounter);
+    int TurnTime ;
+    
+    TurnTime = g_CameraDegCounter - ReturnCount; 
+    
+    if(TurnTime >=0){
+        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
+        wait_ms(Camera_board_wait*TurnTime);
+    }else{
+        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
+        wait_ms(-Camera_board_wait*TurnTime);
+    }
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     
     int SetLoop=0;
@@ -424,7 +463,7 @@
     
     static float TargetDeg = 0;
     
-    TargetDeg = 360*(float)Camera_board_wait*(float)g_CameraDegCounter/(float)Time360;
+    TargetDeg = 360*(float)Camera_board_wait*((float)g_CameraDegCounter - ReturnCount)/(float)Time360;
     
     float HighTargetYaw = TargetDeg + Match_wid;
     float LowTargetYaw = TargetDeg - Match_wid;
@@ -454,6 +493,9 @@
         }
     }
     g_CameraDegCounter = 0;
+    
+    NVIC_EnableIRQ(USART6_IRQn);
+    NVIC_EnableIRQ(USART2_IRQn);
     return;
 }