Diff: main.cpp

Revision:

1:290e621741fd

Parent:

0:84ddd6d354e1

Child:

2:f30666d7838b

diff -r 84ddd6d354e1 -r 290e621741fd main.cpp
--- a/main.cpp	Sat Dec 22 09:27:29 2018 +0000
+++ b/main.cpp	Sat Dec 22 10:41:25 2018 +0000
@@ -22,71 +22,179 @@
 #define MAX_FORWARD     8          //はやい 姿勢修正用
 #define MAX_BACK        9
 
+void getSF_Serial_jevois();
+void getSF_Serial_pi();
+
 PwmOut servoR(PC_6);
 PwmOut servoL(PC_7);
 
-void MoveCansat(int code);
+RawSerial pc(PA_2,PA_3,115200);
+RawSerial pc2(PB_6,PB_7,115200);
+
+char g_landingcommand='X';
+
+void MoveCansat(char g_landingcommand);
 
 int main() {
+    
+    // シリアル通信受信の割り込みイベント登録
+    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) {
-        //printf("Hello World!!");
-        MoveCansat(MOVE_NEUTRAL);
-        printf("STOP\r\n");
-        wait(2);
-        MoveCansat(MOVE_FORWARD);
-        printf("Move Forward\r\n");
-        wait(2);
-        MoveCansat(MOVE_RIGHT);
-        printf("Move right\r\n");
-        wait(2);
-        MoveCansat(MOVE_LEFT);
-        printf("Move left\r\n");
-        wait(2);
+        //pc.printf("Hello World!!");
+        MoveCansat(g_landingcommand);
         }
 }
 
-void MoveCansat(int code)
+void MoveCansat(char g_landingcommand)
 {
-    switch(code){
-        case 0: //MOVE_NEUTRAL 
+    NVIC_DisableIRQ(USART1_IRQn);
+    NVIC_DisableIRQ(USART2_IRQn);
+    switch(g_landingcommand){
+        case 'N': //MOVE_NEUTRAL
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             servoR.pulsewidth_us(servo_NEUTRAL_R);
             servoL.pulsewidth_us(servo_NEUTRAL_L);
             break;
-        case 1: //MOVE_FORWARD
+            
+        case 'F': //MOVE_FORWARD
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             servoR.pulsewidth_us(servo_FORWARD_R);
             servoL.pulsewidth_us(servo_FORWARD_L);
             break;
-        case 2: //MOVE_LEFT 
+            
+        case 'L': //MOVE_LEFT 
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             servoR.pulsewidth_us(servo_slow_FORWARD_R);
             servoL.pulsewidth_us(servo_slow_back_L);
-        case 3: //MOVE_RIGHT 
+            
+        case 'R': //MOVE_RIGHT 
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             servoR.pulsewidth_us(servo_slow_back_R);
             servoL.pulsewidth_us(servo_slow_FORWARD_L);
             break;
-        case 4: //MOVE_BACK 
+            
+        case 'B': //MOVE_BACK 
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             servoR.pulsewidth_us(servo_back_R);
             servoL.pulsewidth_us(servo_back_L);
             break;
-        case 5: //GOAL_FORWARD
+            
+        case 'G': //GOAL_FORWARD
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn); 
             servoR.pulsewidth_us(servo_slow_FORWARD_R);
             servoL.pulsewidth_us(servo_slow_FORWARD_L);
             break;
-        case 6: //GOAL_LEFT 
-            servoR.pulsewidth_us(servo_slow_FORWARD_R);
-            servoL.pulsewidth_us(servo_slow_back_L);
-            break;
-        case 7: //GOAL_RIGHT 
-            servoR.pulsewidth_us(servo_slow_back_R);
-            servoL.pulsewidth_us(servo_slow_FORWARD_L);
+            
+        default :
+            NVIC_EnableIRQ(USART1_IRQn);
+            NVIC_EnableIRQ(USART2_IRQn);  
             break;
-        case 8: //MAX_FORWARD 
-            servoR.pulsewidth_us(servo_FORWARD_R);
-            servoL.pulsewidth_us(servo_FORWARD_L);
-            break;
-        case 9: //MAX_BACK 
-            servoR.pulsewidth_us(servo_back_R);
-            servoL.pulsewidth_us(servo_back_L);
-            break;
+     
     }
     return;
-}
\ No newline at end of file
+}
+
+void getSF_Serial_jevois(){
+
+
+    static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
+    
+    static int bufcounter=0;
+        
+       
+
+    if(pc.readable()) {    // 受信確認
+        
+        SFbuf[bufcounter] = pc.getc();    // 1文字取り出し
+        if(SFbuf[0]!='S'){
+             //pc.printf("x");
+             return;
+        }  
+        
+                
+        
+        //pc.printf("%c",SFbuf[bufcounter]);
+        
+        if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
+            
+        if(bufcounter==5 && SFbuf[4]=='F'){
+                
+            g_landingcommand = SFbuf[1];
+            //pc.printf("%c",g_landingcommand);
+            //wait_ms(20);
+            //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
+            bufcounter = 0;
+            memset(SFbuf, 0, sizeof(SFbuf));
+            NVIC_ClearPendingIRQ(USART2_IRQn);
+            //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
+        }
+            
+        else if(bufcounter>=5){
+            //pc.printf("Communication Falsed.\r\n");
+            memset(SFbuf, 0, sizeof(SFbuf));
+            bufcounter = 0;
+            NVIC_ClearPendingIRQ(USART2_IRQn);
+        }
+    }
+                    
+
+}
+
+
+void getSF_Serial_pi(){
+
+
+    static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
+    
+    static int bufcounter=0;
+        
+       
+
+    if(pc2.readable()) {    // 受信確認
+        
+        SFbuf[bufcounter] = pc2.getc();    // 1文字取り出し
+        if(SFbuf[0]!='S'){
+             //pc.printf("x");
+             return;
+        }  
+        
+                
+        
+        //pc.printf("%c",SFbuf[bufcounter]);
+        
+        if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
+            
+        if(bufcounter==5 && SFbuf[4]=='F'){
+                
+            g_landingcommand = SFbuf[1];
+            //pc.printf("%c",g_landingcommand);
+            //wait_ms(20);
+            //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
+            bufcounter = 0;
+            memset(SFbuf, 0, sizeof(SFbuf));
+            NVIC_ClearPendingIRQ(USART2_IRQn);
+            //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
+        }
+            
+        else if(bufcounter>=5){
+            //pc.printf("Communication Falsed.\r\n");
+            memset(SFbuf, 0, sizeof(SFbuf));
+            bufcounter = 0;
+            NVIC_ClearPendingIRQ(USART2_IRQn);
+        }
+    }
+                    
+
+}
+