File content as of revision 1:290e621741fd:

#include "mbed.h"

#define servo_NEUTRAL_R    1460
#define servo_NEUTRAL_L    1460
#define servo_FORWARD_R    1860
#define servo_FORWARD_L    1860
#define servo_back_R       1060
#define servo_back_L       1060
#define servo_slow_FORWARD_R    1560
#define servo_slow_FORWARD_L    1560
#define servo_slow_back_R       1360
#define servo_slow_back_L       1360

#define MOVE_NEUTRAL    0
#define MOVE_FORWARD    1   
#define MOVE_LEFT       2
#define MOVE_RIGHT      3
#define MOVE_BACK       4
#define GOAL_FORWARD    5          //ゴール付近 ゆっくり
#define GOAL_LEFT       6
#define GOAL_RIGHT      7
#define MAX_FORWARD     8          //はやい 姿勢修正用
#define MAX_BACK        9

void getSF_Serial_jevois();
void getSF_Serial_pi();

PwmOut servoR(PC_6);
PwmOut servoL(PC_7);

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) {
        //pc.printf("Hello World!!");
        MoveCansat(g_landingcommand);
        }
}

void MoveCansat(char g_landingcommand)
{
    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 '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 '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 '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 '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 '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;
            
        default :
            NVIC_EnableIRQ(USART1_IRQn);
            NVIC_EnableIRQ(USART2_IRQn);  
            break;
     
    }
    return;
}

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);
        }
    }
                    

}