File content as of revision 8:d11a59d2a2f1:

#include "mbed.h"
#include "MPU6050_DMP6.h"
#include "HMC5883L.h"

//MPU_check用
#define PI 3.14159265358979

#define servo_NEUTRAL_R    1900
#define servo_NEUTRAL_L    1900
#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 TurnTable_NEUTRAL     1500 //カメラ台座のサーボ
#define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
#define minFocus  1200  //焦点合わせ用サーボの最小値

#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();

//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 CameraDegFlag = false;
bool jevoisFlag = true;

enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度

Timer t;

//PWM pin宣言
PwmOut servoR(PC_6);           //TIM3_CH1 車輪右
PwmOut servoL(PC_7);           //TIM3_CH2 車輪左
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 servoParachute(PA_11);  //skipper USB端子より パラシュート切り離し

/*通信用のpinは
  PA_3(UART2_Rx)   skipperウラ
  PA_12(UART6_Rx)  skipperオモテ USB端子より
  PB_7 (UART1_Rx)  skipperオモテ 6番目 TIM4_CH2
*/

/*超音波はRaspberryPiに積む*/

//外付けコンパス
HMC5883L compass(PB_9, PB_8);    //コンパスセンサー TIM4_CH3とCH4

RawSerial pc(PA_2,PA_3,115200); //uart2
//pa2:UART2_TX,pa3:UART2_RX
RawSerial pc2(PB_6,PB_7,115200); //uart1
//pb6:UART1_TX,pb7:UART1_RX

char g_landingcommand='N';

//MPU_check用
MPU6050DMP6 mpu6050(PC_0,&pc);


int main() {
    
    //MPU_check
    setup();
    
    // シリアル通信受信の割り込みイベント登録
    pc.attach(getSF_Serial_jevois, Serial::RxIrq);
    pc2.attach(getSF_Serial_pi, Serial::RxIrq);
    
    while(1) {
        if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
        else NVIC_DisableIRQ(USART2_IRQn);
        
        if(g_landingcommand!='N') MatchPosition();
        
        if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
        else NVIC_DisableIRQ(USART2_IRQn);
        
        MoveCansat(g_landingcommand); 
        wait_ms(23);
        
        if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
        else NVIC_EnableIRQ(USART2_IRQn);
        }
}


void MoveCansat(char g_landingcommand)
{
    //NVIC_DisableIRQ(USART1_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);
            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); 
            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); 
            
        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); 
            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); 
            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); 
            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);  
            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(){


    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];
            wait_ms(31);//信号が速すぎることによる割り込み防止
            //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(){
    
    //NVIC_DisableIRQ(USART2_IRQn);

    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];
            wait_ms(31);//信号が速すぎることによる割り込み防止
            //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);
        }
    }
    
    //NVIC_EnableIRQ(USART2_IRQn);
                    
}


void setup(){
     
    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(USART2_IRQn,1);
    
    DisplayClock();
    t.start();
    
    pc.printf("MPU calibration start\r\n");
    pc.printf("HMC calibration start\r\n");
    
    float offsetstart = t.read();
    while(t.read() - offsetstart < 26){
        SensingMPU();
        SensingHMC();
        for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
        pc.printf("%3.2f\t",nowAngle_HMC);
        pc.printf("\r\n");
    }
    
    FirstROLL = nowAngle[ROLL];
    FirstPITCH = nowAngle[PITCH];
    nowAngle[ROLL] -=FirstROLL;
    nowAngle[PITCH] -=FirstPITCH;
    FirstYAW = nowAngle[YAW];
    nowAngle[YAW] -= FirstYAW;
    
    g_FirstYAW_HMC = nowAngle_HMC;
    nowAngle_HMC -=g_FirstYAW_HMC;
    if(nowAngle_HMC<0)nowAngle_HMC+=360;
    
    wait(0.2);
    
    pc.printf("All initialized\r\n");
    setupFlag=true;
}


void SensingMPU(){
    //static int16_t deltaT = 0, t_start = 0;
    //t_start = t.read_us();
    
    float rpy[3] = {0};
    static uint16_t count_changeRPY = 0;
    static bool flg_checkoutlier = false;
    NVIC_DisableIRQ(USART1_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    NVIC_DisableIRQ(TIM5_IRQn);
    NVIC_DisableIRQ(EXTI0_IRQn);
    NVIC_DisableIRQ(EXTI9_5_IRQn);

    mpu6050.getRollPitchYaw_Skipper(rpy);
 
    NVIC_EnableIRQ(USART1_IRQn);
    NVIC_EnableIRQ(USART2_IRQn);
    NVIC_EnableIRQ(TIM5_IRQn);
    NVIC_EnableIRQ(EXTI0_IRQn);
    NVIC_EnableIRQ(EXTI9_5_IRQn);
    
    
    //外れ値対策
    for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
    rpy[ROLL] -= FirstROLL;
    rpy[PITCH] -= FirstPITCH;
    if(!setupFlag){
        rpy[YAW] -= FirstYAW;
        }else{
        if(rpy[YAW] >= FirstYAW){
            rpy[YAW] -= FirstYAW;
            }else{
            rpy[YAW] += 360.0f;
            rpy[YAW] -= FirstYAW;
        }
    }
    
    for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
    if(!flg_checkoutlier || count_changeRPY >= 2){
        for(uint8_t i=0; i<3; i++){
            nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
        }
        count_changeRPY = 0;
    }else   count_changeRPY++;
    flg_checkoutlier = false;
    
}


void Init_sensors(){
    if(mpu6050.setup() == -1){
        pc.printf("failed initialize\r\n");
    }
}


void DisplayClock(){
    pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
    pc.printf("HCLK Clock   = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
    pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
    pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
    pc.printf("\r\n");
}
    
void SensingHMC(){
    //static int16_t deltaT = 0, t_start = 0;
    //t_start = t.read_us();
    
    float rpy=0;
    static uint16_t count_changeRPY = 0;
    static bool flg_checkoutlier = false;
    NVIC_DisableIRQ(USART1_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    NVIC_DisableIRQ(TIM5_IRQn);
    NVIC_DisableIRQ(EXTI0_IRQn);
    NVIC_DisableIRQ(EXTI9_5_IRQn);

    rpy= compass.getHeadingXYDeg(20,50);
 
    NVIC_EnableIRQ(USART1_IRQn);
    NVIC_EnableIRQ(USART2_IRQn);
    NVIC_EnableIRQ(TIM5_IRQn);
    NVIC_EnableIRQ(EXTI0_IRQn);
    NVIC_EnableIRQ(EXTI9_5_IRQn);
    
    
    //外れ値対策
    //rpy*= 180.0f/PI;
    if(!setupFlag){
        rpy -= g_FirstYAW_HMC;
        }else{
        if(rpy >= g_FirstYAW_HMC){
            rpy -= g_FirstYAW_HMC;
            }else{
            rpy += 360.0f;
            rpy -= g_FirstYAW_HMC;
        }
    }
    
    if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
    if(!flg_checkoutlier || count_changeRPY >= 2){
        
        nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f;  //2つの移動平均
        
        count_changeRPY = 0;
    }else   count_changeRPY++;
    flg_checkoutlier = false;
    
}
    
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); //HMC
    //pc.printf("\r\n");
    }