Diff: main.cpp

Revision:

38:7c323abc62fb

Parent:

37:8e273677500d

Child:

39:3faddfc87351

--- a/main.cpp	Sun Mar 03 06:54:50 2019 +0000
+++ b/main.cpp	Mon Mar 04 08:07:53 2019 +0000
@@ -74,7 +74,7 @@
 
 bool setupFlag = false;
 bool CameraDegFlag = false;
-bool jevoisFlag = false;
+bool jevoisFlag = true;
 bool FocusFlag = false;
 
 int g_CameraDegCounter = 0;    //カメラの回転数をカウント
@@ -112,7 +112,7 @@
 
 DigitalOut led1(PA_0);  //tanakaOK kimuraは動作不良
 DigitalOut led2(PA_1);  //tanakaOK kimuraOK
-DigitalIn BoardCheck(PB_5);  //使ってないよ
+DigitalIn BoardCheck(PB_8);
 DigitalOut led4(PB_4);  //使ってないよ
 
 
@@ -126,7 +126,7 @@
 
 
 int main() {
-    
+    BoardCheck.mode(PullDown);
     //MPU_check
     setup();
     //コンパスチェック用
@@ -153,9 +153,17 @@
     jevoisFlag = true; 
     g_landingcommand='N';
     wait(5); 
+    MoveCansat(2);
+    wait(10);
     pc.printf("start\r\n");  
     
     
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
+    pc.printf("MatchPosition\r\n");
+    while(BoardCheck == 0){
+            }
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
+
     servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
     wait(1);
     servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
@@ -173,7 +181,7 @@
         
         NVIC_DisableIRQ(USART6_IRQn);
         NVIC_DisableIRQ(USART2_IRQn);
-        if(g_landingcommand!='N'&&g_landingcommand!='P') MatchPosition();
+        if(g_landingcommand!='N') MatchPosition();
         NVIC_EnableIRQ(USART6_IRQn);
         NVIC_EnableIRQ(USART2_IRQn);
         
@@ -294,10 +302,10 @@
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
             wait(5);
-            do{
+            /*do{
                 SensingMPU();
                 wait_ms(30);
-            }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));  
+            }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 1sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -309,10 +317,6 @@
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
             wait(10);
-            do{
-                SensingMPU();
-                wait_ms(30);
-            }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 2sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -325,10 +329,6 @@
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //Camera_board_wait = 
             wait(15);
-            do{
-                SensingMPU();
-                wait_ms(30);
-            }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 3sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -340,10 +340,6 @@
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
             wait(20);
-            do{
-                SensingMPU();
-                wait_ms(30);
-            }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 4sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
@@ -355,10 +351,6 @@
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
             servoL.pulsewidth_us(Servo_high_FORWARD_L);
             wait(25);
-            do{
-                SensingMPU();
-                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);        
             fclose(g_fp);
@@ -374,6 +366,12 @@
         
         case 'P': //jevoisからraspberry piへの切り替え
             jevoisFlag = false;
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
+            //Camera_board_wait = 
+            wait(15);
+            servoR.pulsewidth_us(Servo_NEUTRAL_R);
+            servoL.pulsewidth_us(Servo_NEUTRAL_L);
             servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
             wait_ms((float)Time360/(float)2);
             servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
@@ -475,7 +473,6 @@
 void MatchPosition(){
     NVIC_DisableIRQ(USART6_IRQn);
     NVIC_DisableIRQ(USART2_IRQn);
-    pc.printf("MatchPosition\r\n");
     
     int TurnTime ;
     
@@ -483,24 +480,24 @@
     
     if(TurnTime >=0){
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
-        wait_ms(Camera_board_wait*TurnTime);
-        /*while(BoardCheck == 0){
-            wait_ms(30);
-            }*/
+        //wait_ms(Camera_board_wait*TurnTime);
+        pc.printf("MatchPosition\r\n");
+        while(BoardCheck == 0){
+            }
     }else{
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
-        wait_ms(-Camera_board_wait*TurnTime);
-        /*while(BoardCheck == 0){
-            wait_ms(30);
-            }*/
+        //wait_ms(-Camera_board_wait*TurnTime);
+        pc.printf("MatchPosition\r\n");
+        while(BoardCheck == 0){
+            }
     }
     servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     
-    /*if(jevoisFlag == false ){
+    if(jevoisFlag == false ){
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
         wait_ms((float)Time360/(float)2);
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-        }*/
+        }
     
     int SetLoop=0;