Diff: main.cpp

Revision:

36:e13fca1666a4

Parent:

35:0f89eff001a7

Child:

37:8e273677500d

--- a/main.cpp	Fri Mar 01 19:24:30 2019 +0000
+++ b/main.cpp	Sat Mar 02 00:16:23 2019 +0000
@@ -145,7 +145,7 @@
     pc2.attach(getSF_Serial_pi, Serial::RxIrq);
     t2.start();
     
-    NVIC_DisableIRQ(USART2_IRQn);
+    /*NVIC_DisableIRQ(USART2_IRQn);
     while(g_landingcommand != 'B'){
         wait_ms(30);
         }
@@ -154,7 +154,7 @@
     g_landingcommand='N';
     wait(5); 
     pc.printf("start\r\n");  
-    
+    */
     
     servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
     wait(1);
@@ -279,7 +279,7 @@
         case '1': //MOVE_FORWARD Speed Level 1
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(1);
+            wait(5);
             do{
                 SensingMPU();
                 wait_ms(30);
@@ -294,7 +294,7 @@
         case '2': //MOVE_FORWARD Speed Level 2
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(2);
+            wait(10);
             do{
                 SensingMPU();
                 wait_ms(30);
@@ -309,7 +309,8 @@
         case '3': //MOVE_FORWARD Speed Level 3
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(3);
+            //Camera_board_wait = 
+            wait(15);
             do{
                 SensingMPU();
                 wait_ms(30);
@@ -324,7 +325,7 @@
         case '4': //MOVE_FORWARD Speed Level 4
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(4);
+            wait(20);
             do{
                 SensingMPU();
                 wait_ms(30);
@@ -339,10 +340,10 @@
         case '5': //MOVE_FORWARD Speed Level 5
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
-            wait(5);
+            wait(25);
             do{
                 SensingMPU();
-                wait_ms(30);
+                wait_ms(25);
             }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
             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);        
@@ -404,6 +405,7 @@
                 servoCameradeg.pulsewidth_us(Camera_deg_B-i);
                 servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
                 servoL.pulsewidth_us(Servo_NEUTRAL_L);
+                servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
                 wait_ms(30); 
                 //pc.printf("zoom2\r\n");  
                 if(jevoisFlag == true) FocusAdjust();
@@ -437,6 +439,7 @@
             servoCameradeg.pulsewidth_us(Camera_deg_B-i);
             servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
             servoL.pulsewidth_us(Servo_NEUTRAL_L);
+            servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
             wait_ms(30); 
             //pc.printf("zoom2\r\n");  
             if(jevoisFlag == true) FocusAdjust();
@@ -534,12 +537,14 @@
          servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + Pint_speed);
          servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
          servoL.pulsewidth_us(Servo_NEUTRAL_L);
+         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
          wait(Pint_wait);
          FocusFlag = !FocusFlag;
     }else{
          servoCameraPinto.pulsewidth_us(Focus_NEUTRAL - Pint_speed);
          servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
          servoL.pulsewidth_us(Servo_NEUTRAL_L);
+         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
          wait(Pint_wait);
          FocusFlag = !FocusFlag;
     }