sahaquiel_raspi_uart_test - skipper_raspi_uart_test

Users » imanomadao » Code » sahaquiel_raspi_uart_test
imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test
skipper_raspi_uart_test
Dependencies: mbed MPU6050_2 HMC5883L_4 SDFileSystem3
Diff: main.cpp

Revision:: 35:0f89eff001a7
Parent:: 34:0c2b67b31b12
Child:: 36:e13fca1666a4
--- a/main.cpp	Fri Mar 01 11:03:39 2019 +0000
+++ b/main.cpp	Fri Mar 01 19:24:30 2019 +0000
@@ -74,7 +74,7 @@
 
 bool setupFlag = false;
 bool CameraDegFlag = false;
-bool jevoisFlag = true;
+bool jevoisFlag = false;
 bool FocusFlag = false;
 
 int g_CameraDegCounter = 0;    //カメラの回転数をカウント
@@ -99,8 +99,8 @@
 PwmOut servoTurnTable(PB_0);   //TIM3_CH3 カメラ台回転Servo
 PwmOut servoCameradeg(PB_1);   //TIM3_CH4 カメラ角度調節Servo
 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
-PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
-PwmOut servoGetUP(PC_8);       //skipperウラ 起き上がり動作
+PwmOut servoCameramount(PC_8); //skipperウラ カメラマウント起動
+//PwmOut servoGetUP(PC_8);       //skipperウラ 起き上がり動作
 
 /*通信用のpinは
   PA_3(UART2_Rx)   skipperウラ
@@ -145,16 +145,16 @@
     pc2.attach(getSF_Serial_pi, Serial::RxIrq);
     t2.start();
     
-    /*NVIC_DisableIRQ(USART2_IRQn);
+    NVIC_DisableIRQ(USART2_IRQn);
     while(g_landingcommand != 'B'){
         wait_ms(30);
         }
-        
+    NVIC_EnableIRQ(USART2_IRQn);    
     jevoisFlag = true; 
-    NVIC_EnableIRQ(USART2_IRQn);
+    g_landingcommand='N';
     wait(5); 
     pc.printf("start\r\n");  
-    */
+    
     
     servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
     wait(1);
@@ -165,13 +165,15 @@
         else NVIC_DisableIRQ(USART2_IRQn);
         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);
         
         NVIC_DisableIRQ(USART6_IRQn);
         NVIC_DisableIRQ(USART2_IRQn);
-        if(g_landingcommand!='N') MatchPosition();
+        if(g_landingcommand!='N'&&g_landingcommand!='P') MatchPosition();
         NVIC_EnableIRQ(USART6_IRQn);
         NVIC_EnableIRQ(USART2_IRQn);
         
@@ -204,6 +206,8 @@
             g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
             fprintf(g_fp, "Time = %d min %d sec : Goal is not found.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
             
         case 'Y': //MOVE_FORWARD
@@ -214,26 +218,28 @@
             g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move forward .\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'l': //MOVE_LEFT Low Speed
             servoR.pulsewidth_us(Servo_slow_FORWARD_R);
-            servoL.pulsewidth_us(Servo_NEUTRAL_L);
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            servoL.pulsewidth_us(Servo_NEUTRAL_L); 
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move left low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
             
         case 'L': //MOVE_LEFT High Speed
             servoR.pulsewidth_us(Servo_high_FORWARD_R);
-            servoL.pulsewidth_us(Servo_NEUTRAL_L);
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            servoL.pulsewidth_us(Servo_NEUTRAL_L); 
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
             
         case 'r': //MOVE_RIGHT Low Speed  
@@ -244,6 +250,8 @@
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move right low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
         
         case 'R': //MOVE_RIGHT  High Speed
@@ -254,16 +262,18 @@
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'G': //GOAL_FORWARD
             servoR.pulsewidth_us(Servo_slow_FORWARD_R);
             servoL.pulsewidth_us(Servo_slow_FORWARD_L);
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
             g_fp = fopen( "/sd/Datalog01.txt","a" );
             fprintf(g_fp, "Time = %d min %d sec : Cansat move goal forward mode.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
             fclose(g_fp);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             break;
         
         case '1': //MOVE_FORWARD Speed Level 1
@@ -273,12 +283,12 @@
             do{
                 SensingMPU();
                 wait_ms(30);
-            }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn);  
+            }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);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
         
         case '2': //MOVE_FORWARD Speed Level 2
@@ -289,11 +299,11 @@
                 SensingMPU();
                 wait_ms(30);
             }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
             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);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
         
         case '3': //MOVE_FORWARD Speed Level 3
@@ -303,12 +313,12 @@
             do{
                 SensingMPU();
                 wait_ms(30);
-            }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            }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);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
         
         case '4': //MOVE_FORWARD Speed Level 4
@@ -318,12 +328,12 @@
             do{
                 SensingMPU();
                 wait_ms(30);
-            }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)||(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            }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);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);
             break;
             
         case '5': //MOVE_FORWARD Speed Level 5
@@ -334,11 +344,11 @@
                 SensingMPU();
                 wait_ms(30);
             }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
-            NVIC_EnableIRQ(USART6_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
             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);
+            NVIC_EnableIRQ(USART6_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'M': //MatchPosition
@@ -373,8 +383,12 @@
 }
 
 void MoveCameraBoard(){
+    if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+    else NVIC_DisableIRQ(USART2_IRQn);
     //pc.printf("start\r\n");
     MoveCansat('N');
+    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);
@@ -394,6 +408,10 @@
                 //pc.printf("zoom2\r\n");  
                 if(jevoisFlag == true) FocusAdjust();
                 else wait(1);
+                
+                if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+                else NVIC_DisableIRQ(USART2_IRQn);
+                
                 if(g_landingcommand!='N') return;  
         
         }
@@ -407,6 +425,9 @@
     //pc.printf("zoom1\r\n");
     if(jevoisFlag == true) FocusAdjust();
     else wait(1);
+    
+    if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+    else NVIC_DisableIRQ(USART2_IRQn);
         
     
     if(g_landingcommand!='N') return;
@@ -420,6 +441,10 @@
             //pc.printf("zoom2\r\n");  
             if(jevoisFlag == true) FocusAdjust();
             else wait(1);
+            
+            if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+            else NVIC_DisableIRQ(USART2_IRQn);
+                
             if(g_landingcommand!='N') return;  
     
     }
@@ -483,14 +508,14 @@
         while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
             //MoveCansat('r');
             SensingMPU();
-            wait_ms(30);
+            wait_ms(10);
             pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
         }
     }else{
         while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
             //MoveCansat('r');
             SensingMPU();
-            wait_ms(30);
+            wait_ms(10);
             pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
         }
     }
@@ -502,6 +527,9 @@
 }
 
 void FocusAdjust(){
+    if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
+    else NVIC_DisableIRQ(USART2_IRQn);
+    
     if(FocusFlag == false){
          servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + Pint_speed);
          servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	06 Feb 2020
Imports:	1
Forks:	0
Commits:	43
Dependents:	0
Dependencies:	4
Followers:	1
Diff: main.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
