Diff: main.cpp

Revision:

7:8989a4b84695

Parent:

6:166746820555

Child:

8:d11a59d2a2f1

diff -r 166746820555 -r 8989a4b84695 main.cpp
--- a/main.cpp	Fri Feb 08 10:39:17 2019 +0000
+++ b/main.cpp	Sat Feb 09 06:23:48 2019 +0000
@@ -15,6 +15,9 @@
 #define servo_slow_FORWARD_L    1560
 #define servo_slow_back_R       1360
 #define servo_slow_back_L       1360
+#define TurnTable_NEUTRAL     1500 //カメラ台座のサーボ
+#define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
+#define minFocus  1200  //焦点合わせ用サーボの最小値
 
 #define MOVE_NEUTRAL    0
 #define MOVE_FORWARD    1   
@@ -31,19 +34,25 @@
 void getSF_Serial_jevois();
 void getSF_Serial_pi();
 
-
 //MPU_check用
 void SensingMPU();
+
+void MoveCansat(char g_landingcommand);
 void setup();
 void Init_sensors();
 void DisplayClock();
 void DebugPrint();
 void SensingHMC();
+void MoveCameraBoard();
+void MatchPosition();
+void FocusAdjust();
 
 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
 
-bool setupFlag=false;
+bool setupFlag = false;
+bool CameraDegFlag = false;
+bool jevoisFlag = true;
 
 enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度
 
@@ -77,8 +86,6 @@
 
 char g_landingcommand='N';
 
-void MoveCansat(char g_landingcommand);
-
 //MPU_check用
 MPU6050DMP6 mpu6050(PC_0,&pc);
 
@@ -92,18 +99,14 @@
     pc.attach(getSF_Serial_jevois, Serial::RxIrq);
     pc2.attach(getSF_Serial_pi, Serial::RxIrq);
     
-    NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
-    NVIC_SetPriority(USART2_IRQn,1);
-    
     while(1) {
-        //pc.printf("Hello World!!");
-        MoveCansat(g_landingcommand);
-        
-        SensingMPU();
-        SensingHMC();
-        DebugPrint();
-        
+        if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
+        else NVIC_DisableIRQ(USART2_IRQn);
+        if(g_landingcommand!='N') MatchPosition();
+        MoveCansat(g_landingcommand); 
         wait_ms(23);
+        if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
+        else NVIC_EnableIRQ(USART2_IRQn);
         }
 }
 
@@ -111,58 +114,123 @@
 void MoveCansat(char g_landingcommand)
 {
     //NVIC_DisableIRQ(USART1_IRQn);
-    NVIC_DisableIRQ(USART2_IRQn);
+    //NVIC_DisableIRQ(USART2_IRQn);
     switch(g_landingcommand){
         case 'N': //MOVE_NEUTRAL 
             servoR.pulsewidth_us(servo_NEUTRAL_R);
             servoL.pulsewidth_us(servo_NEUTRAL_L);
             //NVIC_EnableIRQ(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn);
+            //NVIC_EnableIRQ(USART2_IRQn);
             break;
             
         case 'Y': //MOVE_FORWARD
             servoR.pulsewidth_us(servo_FORWARD_R);
             servoL.pulsewidth_us(servo_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            //NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'L': //MOVE_LEFT 
             servoR.pulsewidth_us(servo_slow_FORWARD_R);
             servoL.pulsewidth_us(servo_slow_back_L);
             //NVIC_EnableIRQ(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            //NVIC_EnableIRQ(USART2_IRQn); 
             
         case 'R': //MOVE_RIGHT  
             servoR.pulsewidth_us(servo_slow_back_R);
             servoL.pulsewidth_us(servo_slow_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            //NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'B': //MOVE_BACK 
             servoR.pulsewidth_us(servo_back_R);
             servoL.pulsewidth_us(servo_back_L);
             //NVIC_EnableIRQ(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_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(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn); 
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
+            
+        case 'J': //MOVE_FORWARD Tim
+            servoR.pulsewidth_us(servo_FORWARD_R);
+            servoL.pulsewidth_us(servo_FORWARD_L);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            wait(5);
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
+            
+        case 'M': //MatchPosition
+            servoR.pulsewidth_us(MatchSpeed);
+            //NVIC_EnableIRQ(USART1_IRQn);
+            //NVIC_EnableIRQ(USART2_IRQn); 
+            break;
+        
+        case 'T': //jevoisからraspberry piへの切り替え
+            jevoisFlag = false;
+            //NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         default :
             //NVIC_EnableIRQ(USART1_IRQn);
-            NVIC_EnableIRQ(USART2_IRQn);  
+            //NVIC_EnableIRQ(USART2_IRQn);  
             break;
      
     }
+    
     return;
 }
 
+void MoveCameraBoard(){
+    MoveCansat('N');
+    servoTurnTable.pulsewidth_us(2000);
+    wait_ms(300);
+    servoTurnTable.pulsewidth_us(TurnTable_NEUTRAL);
+    if(jevoisFlag == true) FocusAdjust();
+    else wait(1);
+    
+    if(g_landingcommand!='N') return;
+    if(!CameraDegFlag){
+        servoCameradeg.pulsewidth_us(1800);
+        CameraDegFlag=!CameraDegFlag;
+        }else{
+        servoCameradeg.pulsewidth_us(1500);
+        CameraDegFlag = !CameraDegFlag;
+    }
+        
+    if(jevoisFlag == true) FocusAdjust();
+    else wait(1);
+    
+    return;
+}
+
+void MatchPosition(){
+    SensingMPU();
+    SensingHMC();
+    DebugPrint();
+    
+    while(nowAngle_HMC <= nowAngle[YAW]+5 && nowAngle_HMC >= nowAngle[YAW]-5){
+        MoveCansat('M');
+    }
+    return;
+}
+
+void FocusAdjust(){
+    servoCameraPinto.pulsewidth_us(minFocus);
+    
+    for(int i=0; i<400; i++){
+        servoCameraPinto.pulsewidth_us(minFocus+i);
+        wait_ms(30);
+        if(g_landingcommand!='N') return;
+    }
+  return;      
+}    
+
 void getSF_Serial_jevois(){
 
 
@@ -213,7 +281,7 @@
 
 void getSF_Serial_pi(){
     
-    NVIC_DisableIRQ(USART2_IRQn);
+    //NVIC_DisableIRQ(USART2_IRQn);
 
     static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
     
@@ -256,7 +324,7 @@
         }
     }
     
-    NVIC_EnableIRQ(USART2_IRQn);
+    //NVIC_EnableIRQ(USART2_IRQn);
                     
 }
 
@@ -266,11 +334,15 @@
     Init_sensors();
     //switch2.rise(ResetTrim);
     
-    NVIC_SetPriority(USART1_IRQn,0);
-    NVIC_SetPriority(EXTI0_IRQn,1);
-    NVIC_SetPriority(TIM5_IRQn,2);
-    NVIC_SetPriority(EXTI9_5_IRQn,3);
-    NVIC_SetPriority(USART2_IRQn,4);
+    //NVIC_SetPriority(USART1_IRQn,0);
+    //NVIC_SetPriority(EXTI0_IRQn,1);
+    //NVIC_SetPriority(TIM5_IRQn,2);
+    //NVIC_SetPriority(EXTI9_5_IRQn,3);
+    //NVIC_SetPriority(USART2_IRQn,4);
+    
+    NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
+    NVIC_SetPriority(USART2_IRQn,1);
+    
     DisplayClock();
     t.start();
     
@@ -308,7 +380,7 @@
     //static int16_t deltaT = 0, t_start = 0;
     //t_start = t.read_us();
     
-    float rpy[3] = {0}, oldrpy[3] = {0};
+    float rpy[3] = {0};
     static uint16_t count_changeRPY = 0;
     static bool flg_checkoutlier = false;
     NVIC_DisableIRQ(USART1_IRQn);
@@ -372,7 +444,7 @@
     //static int16_t deltaT = 0, t_start = 0;
     //t_start = t.read_us();
     
-    float rpy=0, oldrpy=0;
+    float rpy=0;
     static uint16_t count_changeRPY = 0;
     static bool flg_checkoutlier = false;
     NVIC_DisableIRQ(USART1_IRQn);
@@ -416,7 +488,7 @@
     
 void DebugPrint(){
     //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
-    pc.printf("%3.2f\t",nowAngle[2]);
-    pc.printf("%3.2f\t",nowAngle_HMC);
-    pc.printf("\r\n");
+    //pc.printf("%3.2f\t",nowAngle[2]);
+    //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
+    //pc.printf("\r\n");
     }
\ No newline at end of file