Diff: main.cpp

Revision:

41:d142afd3c2f7

Parent:

40:917b50b9e863

Child:

42:859ba941fb7e

--- a/main.cpp	Tue Mar 05 16:46:01 2019 +0000
+++ b/main.cpp	Thu Mar 07 17:50:58 2019 +0000
@@ -26,7 +26,7 @@
 #define match_wid                 5
 #define camera_board_wait       100
 
-#define ReturnCount             2
+#define ReturnCount             4
 
 #define MOVE_NEUTRAL    0
 #define MOVE_FORWARD    1    
@@ -134,7 +134,7 @@
         //SensingMPU();
         SensingHMC();
         //pc.printf("%3.2f\t",nowAngle[2]);
-        pc.printf("%3.2f\t",nowAngle_HMC); //HMC
+       // pc.printf("%3.2f\t",nowAngle_HMC); //HMC
         pc.printf("\r\n");
         wait_ms(30);
     }*/
@@ -160,14 +160,14 @@
     servoR.pulsewidth_us(Servo_NEUTRAL_R);
     servoL.pulsewidth_us(Servo_NEUTRAL_L);
     wait(10);
-    pc.printf("start\r\n"); 
+    //pc.printf("start\r\n"); 
     g_fp = fopen( "/sd/Datalog01.txt","a" );
     fprintf(g_fp, "Time = %d min %d sec : Start sarching target .\r\n",(int)t2.read()/60,(int)t2.read()%60);        
     fclose(g_fp); 
     */
     
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
-    pc.printf("MatchPosition\r\n");
+    //pc.printf("MatchPosition\r\n");
     while(BoardCheck == 0){
             }
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
@@ -179,13 +179,13 @@
     while(1) {
         if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
         else NVIC_DisableIRQ(USART2_IRQn);
-        pc.printf("Move Camera Board\r\n");
+        //pc.printf("Move Camera Board\r\n");
         MoveCameraBoard();
         if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
         else NVIC_DisableIRQ(USART2_IRQn);
         
-        pc.printf("Position\r\n");
-        pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
+        //pc.printf("Position\r\n");
+        //pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
         
         NVIC_DisableIRQ(USART6_IRQn);
         NVIC_DisableIRQ(USART2_IRQn);
@@ -196,13 +196,13 @@
         if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
         else NVIC_DisableIRQ(USART2_IRQn);
         
-        pc.printf("Move Cansat\r\n");
+        //pc.printf("Move Cansat\r\n");
         MoveCansat(g_landingcommand); 
         if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
         else NVIC_DisableIRQ(USART2_IRQn);
         wait_ms(23);
         
-        pc.printf("finish\r\n");
+        //pc.printf("finish\r\n");
         if(jevoisFlag==true) NVIC_EnableIRQ(USART6_IRQn);
         else NVIC_EnableIRQ(USART2_IRQn);
         }
@@ -336,7 +336,7 @@
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //Camera_board_wait = 
-            wait(15);
+            wait(30); //6m
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 3sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -347,7 +347,7 @@
         case '4': //MOVE_FORWARD Speed Level 4
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(20);
+            wait(50);//10m
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 4sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -358,7 +358,7 @@
         case '5': //MOVE_FORWARD Speed Level 5
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(25);
+            wait(100);//20m
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 5sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -407,12 +407,15 @@
     if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
     else NVIC_DisableIRQ(USART2_IRQn);
     //pc.printf("start\r\n");
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     MoveCansat('N');
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
     else NVIC_DisableIRQ(USART2_IRQn);
     //pc.printf("ok\r\n");
     g_landingcommand='N';
     servoCameradeg.pulsewidth_us(Camera_deg_B);
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     wait_ms(30);
     if(g_CameraDegCounter==0){
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
@@ -447,9 +450,11 @@
     //pc.printf("zoom1\r\n");
     if(jevoisFlag == true) FocusAdjust();
     else wait(1);
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     
     if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
     else NVIC_DisableIRQ(USART2_IRQn);
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
         
     
     if(g_landingcommand!='N') return;
@@ -473,10 +478,10 @@
     }
     
     
-    pc.printf("g_CameraDegCounter = %d\r\n",g_CameraDegCounter);
+    //pc.printf("g_CameraDegCounter = %d\r\n",g_CameraDegCounter);
     
     //ターゲットを発見できなかった場合
-    if(g_CameraDegCounter-2 > (float)Time360/(float)Camera_board_wait){
+    if(g_CameraDegCounter-ReturnCount > (float)Time360/(float)Camera_board_wait){
         
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
         wait(1);
@@ -504,6 +509,10 @@
     NVIC_DisableIRQ(USART6_IRQn);
     NVIC_DisableIRQ(USART2_IRQn);
     
+    g_fp = fopen( "/sd/Datalog01.txt","a" );
+    fprintf(g_fp, "Time = %d min %d sec : Get signal!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
+    fclose(g_fp);
+    
     int TurnTime ;
     
     TurnTime = g_CameraDegCounter - ReturnCount; 
@@ -511,13 +520,13 @@
     if(TurnTime >=0){
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
         //wait_ms(Camera_board_wait*TurnTime);
-        pc.printf("MatchPosition\r\n");
+        //pc.printf("MatchPosition\r\n");
         while(BoardCheck == 0){
             }
     }else{
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
         //wait_ms(-Camera_board_wait*TurnTime);
-        pc.printf("MatchPosition\r\n");
+        //pc.printf("MatchPosition\r\n");
         while(BoardCheck == 0){
             }
     }
@@ -557,7 +566,7 @@
     if(LowTargetYaw >= 360.0f) LowTargetYaw = LowTargetYaw - 360.0f;
     if(LowTargetYaw < 0.0f) LowTargetYaw = LowTargetYaw + 360.0f;
     
-    pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
+    //pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
     
     MoveCansat('r');
     
@@ -566,14 +575,14 @@
             //MoveCansat('r');
             SensingMPU();
             wait_ms(10);
-            pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
+            //pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
         }
     }else{
         while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
             //MoveCansat('r');
             SensingMPU();
             wait_ms(10);
-            pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
+            //pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
         }
     }
     g_CameraDegCounter = 0;
@@ -632,7 +641,7 @@
         
                 
         
-        pc.printf("%c",SFbuf[bufcounter]);
+        //pc.printf("%c",SFbuf[bufcounter]);
         
         if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
             
@@ -681,7 +690,7 @@
         
                 
         
-        pc.printf("%c",SFbuf[bufcounter]);
+        //pc.printf("%c",SFbuf[bufcounter]);
         
         if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;