File content as of revision 38:7c323abc62fb:

#include "mbed.h"
#include "MPU6050_DMP6.h"
#include "HMC5883L.h"
#include "SDFileSystem.h"
#include "SkipperSv2.h"
#include "falfalla.h"

//MPU_check用
#define PI 3.14159265358979

#define servo_NEUTRAL_R         1614
#define servo_NEUTRAL_L         1621
#define servo_slow_FORWARD_R    1560
#define servo_slow_FORWARD_L    1560
#define servo_high_FORWARD_R    1860
#define servo_high_FORWARD_L    1860
#define turntable_NEUTRAL       1180        //カメラ台座のサーボ
#define matchspeed              1500 + 100  //カメラと方向を合わせるときの車輪の速度
#define focus_NEUTRAL           1455        //焦点合わせ用サーボ
#define camera_deg_A            1400        //カメラ角度調整
#define camera_deg_B            1800
#define pint_speed              25
#define pint_wait               3
#define turntable_speed         1800 
#define time360                 110
#define match_wid                 5
#define camera_board_wait       100

#define ReturnCount             2

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

//sd設定
int GetParameter(FILE *fp, const char *paramName,char parameter[]);

int SetOptions(int *Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
            int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
            int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
            int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
            int *Camera_deg_A, int *Camera_deg_B,
            int *Pint_speed, float *Pint_wait,
            int *Turntable_speed,
            int *Time360, int *Match_wid,
            int *Camera_board_wait
            );

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;
bool FocusFlag = false;

int g_CameraDegCounter = 0;    //カメラの回転数をカウント

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

Timer t;
Timer t2;

FILE *g_fp;

SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");

RawSerial pc(PA_2,PA_3,115200); //uart2 jevoiis
//pa2:UART2_TX,pa3:UART2_RX
RawSerial pc2(PA_11,PA_12,115200); //uart1 raspberry
//pb6:UART1_TX,pb7:UART1_RX

//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(PC_8); //skipperウラ カメラマウント起動
//PwmOut servoGetUP(PC_8);       //skipperウラ 起き上がり動作

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

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

DigitalOut led1(PA_0);  //tanakaOK kimuraは動作不良
DigitalOut led2(PA_1);  //tanakaOK kimuraOK
DigitalIn BoardCheck(PB_8);
DigitalOut led4(PB_4);  //使ってないよ


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

char g_landingcommand='N';

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


int main() {
    BoardCheck.mode(PullDown);
    //MPU_check
    setup();
    //コンパスチェック用
    /*while(1){
        //SensingMPU();
        SensingHMC();
        //pc.printf("%3.2f\t",nowAngle[2]);
        pc.printf("%3.2f\t",nowAngle_HMC); //HMC
        pc.printf("\r\n");
        wait_ms(30);
    }*/
    
    
    // シリアル通信受信の割り込みイベント登録
    pc.attach(getSF_Serial_jevois, Serial::RxIrq);
    pc2.attach(getSF_Serial_pi, Serial::RxIrq);
    t2.start();
    
    NVIC_DisableIRQ(USART2_IRQn);
    while(g_landingcommand != 'B'){
        wait_ms(30);
        }
    NVIC_EnableIRQ(USART2_IRQn);    
    jevoisFlag = true; 
    g_landingcommand='N';
    wait(5); 
    MoveCansat(2);
    wait(10);
    pc.printf("start\r\n");  
    
    
    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
    pc.printf("MatchPosition\r\n");
    while(BoardCheck == 0){
            }
    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);

    servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
    wait(1);
    servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
    
    while(1) {
        if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
        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();
        NVIC_EnableIRQ(USART6_IRQn);
        NVIC_EnableIRQ(USART2_IRQn);
        
        if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
        else NVIC_DisableIRQ(USART2_IRQn);
        
        pc.printf("Move Cansat\r\n");
        MoveCansat(g_landingcommand); 
        if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
        else NVIC_DisableIRQ(USART2_IRQn);
        wait_ms(23);
        
        pc.printf("finish\r\n");
        if(jevoisFlag==true) NVIC_EnableIRQ(USART6_IRQn);
        else NVIC_EnableIRQ(USART2_IRQn);
        }
}


void MoveCansat(char a)
{
    NVIC_DisableIRQ(USART6_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    switch(a){
        case 'N': //MOVE_NEUTRAL 
            servoR.pulsewidth_us(Servo_NEUTRAL_R);
            servoL.pulsewidth_us(Servo_NEUTRAL_L);
            NVIC_EnableIRQ(USART6_IRQn);
            NVIC_EnableIRQ(USART2_IRQn);
            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
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_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 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); 
            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); 
            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  
            servoR.pulsewidth_us(Servo_NEUTRAL_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 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
            servoR.pulsewidth_us(Servo_NEUTRAL_R);
            servoL.pulsewidth_us(Servo_high_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 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);
            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 '0': //MOVE_FORWARD Speed Level 1
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            wait(5);  
            g_fp = fopen( "/sd/Datalog01.txt","a" );
            fprintf(g_fp, "Time = %d min %d sec : Finish!.\r\n",(int)t2.read()/60,(int)t2.read()%60);        
            fclose(g_fp);
            servoR.pulsewidth_us(Servo_NEUTRAL_R);
            servoL.pulsewidth_us(Servo_NEUTRAL_L);
            exit(0);
            NVIC_EnableIRQ(USART6_IRQn);
            NVIC_EnableIRQ(USART2_IRQn);
            break;
        
        case '1': //MOVE_FORWARD Speed Level 1
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            wait(5);
            /*do{
                SensingMPU();
                wait_ms(30);
            }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
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            wait(10);
            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
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            //Camera_board_wait = 
            wait(15);
            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
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            wait(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
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            wait(25);
            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
            servoR.pulsewidth_us(Matchspeed);
            NVIC_EnableIRQ(USART6_IRQn);
            NVIC_EnableIRQ(USART2_IRQn); 
            break;
        
        case 'P': //jevoisからraspberry piへの切り替え
            jevoisFlag = false;
            servoR.pulsewidth_us(Servo_high_FORWARD_R);
            servoL.pulsewidth_us(Servo_high_FORWARD_L);
            //Camera_board_wait = 
            wait(15);
            servoR.pulsewidth_us(Servo_NEUTRAL_R);
            servoL.pulsewidth_us(Servo_NEUTRAL_L);
            servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
            wait_ms((float)Time360/(float)2);
            servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
            NVIC_EnableIRQ(USART6_IRQn);
            NVIC_EnableIRQ(USART2_IRQn); 
            break;
        
        case 'B':
            /*RasPiからの超音波判定(プログラムスタート部)*/
            NVIC_EnableIRQ(USART6_IRQn);
            NVIC_EnableIRQ(USART2_IRQn); 
            break;
            
        default :
            NVIC_EnableIRQ(USART6_IRQn);
            NVIC_EnableIRQ(USART2_IRQn); 
            break;
     
    }
    
    return;
}

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);
    wait_ms(30);
    if(g_CameraDegCounter==0){
        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
        wait_ms(Camera_board_wait*ReturnCount);
        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
    
    
        for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
                
                servoCameradeg.pulsewidth_us(Camera_deg_B-i);
                servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
                servoL.pulsewidth_us(Servo_NEUTRAL_L);
                servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
                wait_ms(30); 
                //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;  
        
        }
        
    }
    
    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
    wait_ms(Camera_board_wait);
    g_CameraDegCounter++;
    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
    //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;
    
    for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
            
            servoCameradeg.pulsewidth_us(Camera_deg_B-i);
            servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
            servoL.pulsewidth_us(Servo_NEUTRAL_L);
            servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
            wait_ms(30); 
            //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;  
    
    }
    
    
    pc.printf("g_CameraDegCounter = %d\r\n",g_CameraDegCounter);
    //pc.printf("Move Board Finish\r\n");  
    return;
}

void MatchPosition(){
    NVIC_DisableIRQ(USART6_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    
    int TurnTime ;
    
    TurnTime = g_CameraDegCounter - ReturnCount; 
    
    if(TurnTime >=0){
        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
        //wait_ms(Camera_board_wait*TurnTime);
        pc.printf("MatchPosition\r\n");
        while(BoardCheck == 0){
            }
    }else{
        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
        //wait_ms(-Camera_board_wait*TurnTime);
        pc.printf("MatchPosition\r\n");
        while(BoardCheck == 0){
            }
    }
    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
    
    if(jevoisFlag == false ){
        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
        wait_ms((float)Time360/(float)2);
        servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
        }
    
    int SetLoop=0;
    
    while(SetLoop<30){
    SensingMPU();
    wait_ms(10);
    //SensingHMC();
    //wait_ms(20);
    //DebugPrint();
    SetLoop++;
    //pc.printf("nowAngle_HMC=%f\tMPU=%f\r\n",nowAngle_HMC,nowAngle[YAW]);
    
    }
    
    static float TargetDeg = 0;
    
    TargetDeg = 360*(float)Camera_board_wait*((float)g_CameraDegCounter - ReturnCount)/(float)Time360;
    
    if(TargetDeg > 360)TargetDeg = TargetDeg - 360;
    if(TargetDeg < 0)TargetDeg = TargetDeg + 360; 
    
    float HighTargetYaw = nowAngle[YAW] + TargetDeg + Match_wid;
    float LowTargetYaw = nowAngle[YAW] + TargetDeg - Match_wid;
    
    
    if(HighTargetYaw >= 360.0f) HighTargetYaw = HighTargetYaw - 360.0f;
    if(LowTargetYaw >= 360.0f) LowTargetYaw = LowTargetYaw - 360.0f;
    if(LowTargetYaw < 0.0f) LowTargetYaw = LowTargetYaw + 360.0f;
    
    pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
    
    MoveCansat('r');
    
    if(HighTargetYaw-LowTargetYaw<0){
        while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
            //MoveCansat('r');
            SensingMPU();
            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(10);
            pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
        }
    }
    g_CameraDegCounter = 0;
    
    NVIC_EnableIRQ(USART6_IRQn);
    NVIC_EnableIRQ(USART2_IRQn);
    return;
}

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
         servoL.pulsewidth_us(Servo_NEUTRAL_L);
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
         wait(Pint_wait);
         FocusFlag = !FocusFlag;
    }else{
         servoCameraPinto.pulsewidth_us(Focus_NEUTRAL - Pint_speed);
         servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
         servoL.pulsewidth_us(Servo_NEUTRAL_L);
         servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
         wait(Pint_wait);
         FocusFlag = !FocusFlag;
    }
    servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
      
  return;      
}     

void getSF_Serial_jevois(){
    
    //pc.printf("jevois\r\n");


    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(){
    
    led1 = 1;
    led2 = 1;
    //led3 = 1;
    led4 = 1;
    
    servoR.pulsewidth_us(Servo_NEUTRAL_R);       //servo initialize
    servoL.pulsewidth_us(Servo_NEUTRAL_L);
    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
    servoCameradeg.pulsewidth_us(Camera_deg_B);
    servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
     
    SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
               &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
               &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
               &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
               &Camera_deg_A, &Camera_deg_B,
               &Pint_speed,&Pint_wait,
               &Turntable_speed,
               &Time360, &Match_wid,
               &Camera_board_wait
               );
     
    Init_sensors();
    //switch2.rise(ResetTrim);
    
    //NVIC_SetPriority(USART6_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(USART6_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("\t%3.2f\t",nowAngle_HMC);
        pc.printf("\r\n");
        led1 = !led1;
        led2 = !led2;
        //led3 = !led3;
        led4 = !led4;
    }
    
    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;
    
    led1 = 0;
    led2 = 0;
    //led3 = 0;
    led4 = 0;
        
    wait(0.2);
    
    
    g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
    if( g_fp == NULL ) {
        pc.printf( "File open error!!!\r\n" );
    }
    fprintf(g_fp,"\r\n-------------------------\r\n");
    fprintf(g_fp,"All initialized.\r\n");
    fclose( g_fp );
    
    
    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(USART6_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    NVIC_DisableIRQ(TIM5_IRQn);
    NVIC_DisableIRQ(EXTI0_IRQn);
    NVIC_DisableIRQ(EXTI9_5_IRQn);

    mpu6050.getRollPitchYaw_Skipper(rpy);
 
    NVIC_EnableIRQ(USART6_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/(float)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++){
            if(rpy[i] > nowAngle[i]-180 && rpy[i] < nowAngle[i]+180){
                nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
            }else{
                nowAngle[i] = rpy[i];
            }
        }
        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(USART6_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    //NVIC_DisableIRQ(TIM5_IRQn);
    //NVIC_DisableIRQ(EXTI0_IRQn);
    //NVIC_DisableIRQ(EXTI9_5_IRQn);

    rpy= compass.getHeadingXYDeg(Time360,Match_wid);
 
    NVIC_EnableIRQ(USART6_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;
    
}*/

int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
               int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
               int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
               int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
               int *Camera_deg_A, int *Camera_deg_B,
               int *Pint_speed, float *Pint_wait,
               int *Turntable_speed,
               int *Time360, int *Match_wid,
               int *Camera_board_wait
               ){

    pc.printf("SDsetup start.\r\n");    
    
    FILE *fp;
    char parameter[20]; //文字列渡す用の配列
    int SDerrorcount = 0;  //取得できなかった数を返す
    const char *paramNames[] = { 
        "SERVO_NEUTRAL_R",
        "SERVO_NEUTRAL_L",
        "SERVO_HIGH_FORWARD_R",
        "SERVO_HIGH_FORWARD_L",
        "SERVO_SLOW_FORWARD_R",
        "SERVO_SLOW_FORWARD_L",
        "TURNTABLE_NEUTRAL",
        "MATCH_SPEED",
        "FOCUS_NEUTRAL",
        "CAMERA_DEG_A",
        "CAMERA_DEG_B",
        "PINT_SPEED",
        "PINT_WAIT",
        "TURNTABLE_SPEED",
        "TIME360",
        "MATCH_WID",
        "CAMERA_BOARD_WAIT"
    };

    fp = fopen("/sd/option.txt","r");
    
    if(fp != NULL){ //開けたら
        pc.printf("File was openned.\r\n");
        if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
        else{                                        *Servo_NEUTRAL_R = servo_NEUTRAL_R;
                                                     SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
        else{                                        *Servo_NEUTRAL_L = servo_NEUTRAL_L;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter);
        else{                                        *Servo_high_FORWARD_R = servo_high_FORWARD_R;
                                                     SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
        else{                                        *Servo_high_FORWARD_L = servo_high_FORWARD_L;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
        else{                                        *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
        else{                                        *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
                                                      SDerrorcount++;
        }
        
        if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
        else{                                         *Turntable_NEUTRAL = turntable_NEUTRAL;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
        else{                                         *Matchspeed = matchspeed;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
        else{                                         *Focus_NEUTRAL = focus_NEUTRAL;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
        else{                                         *Camera_deg_A = camera_deg_A;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
        else{                                         *Camera_deg_B = camera_deg_B;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
        else{                                         *Pint_speed = pint_speed;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
        else{                                         *Pint_wait = pint_wait;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
        else{                                         *Turntable_speed = turntable_speed;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[14],parameter)) *Time360 = atof(parameter);
        else{                                         *Time360 = time360;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[15],parameter)) *Match_wid = atof(parameter);
        else{                                         *Match_wid = match_wid;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[16],parameter)) *Camera_board_wait = atof(parameter);
        else{                                         *Camera_board_wait = camera_board_wait;
                                                      SDerrorcount++;
        }
        fclose(fp);

    }else{  //ファイルがなかったら
        pc.printf("fp was null.\r\n");
        
        *Servo_NEUTRAL_R = servo_NEUTRAL_R;
        *Servo_NEUTRAL_L = servo_NEUTRAL_L;
        *Servo_high_FORWARD_R = servo_high_FORWARD_R;
        *Servo_high_FORWARD_L = servo_high_FORWARD_L;
        *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
        *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
        *Turntable_NEUTRAL = turntable_NEUTRAL;
        *Matchspeed = matchspeed;
        *Focus_NEUTRAL = focus_NEUTRAL;
        *Camera_deg_A = camera_deg_A;
        *Camera_deg_B = camera_deg_B;
        *Pint_speed = pint_speed;
        *Pint_wait = pint_wait;
        *Turntable_speed = turntable_speed;
        *Time360 = time360;
        *Match_wid = match_wid;
        *Camera_board_wait = camera_board_wait;
        SDerrorcount = -1;
    }
    pc.printf("SDsetup finished.\r\n");
    if(SDerrorcount == 0)  pc.printf("setting option is success\r\n");
    else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
    else if(SDerrorcount > 0)  pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 
    
    pc.printf("Servo_NEUTRAL_R = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
    pc.printf("Servo_high_FORWARD_R = %d, Servo_high_FORWARD_L = %d\r\n", *Servo_high_FORWARD_R, *Servo_high_FORWARD_L);
    pc.printf("Servo_slow_FORWARD_R = %d, Servo_slow_FORWARD_L = %d\r\n", *Servo_slow_FORWARD_R, *Servo_slow_FORWARD_L);
    pc.printf("Turntable_NEUTRAL = %d, Matchspeed =  %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
    pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
    pc.printf("Camera_deg_A = %d, Camera_deg_B =  %d\r\n", *Camera_deg_A, *Camera_deg_B);
    pc.printf("Pint_speed = %d, Pint_wait =  %f\r\n", *Pint_speed, *Pint_wait);
    pc.printf("Turntable_speed = %d\r\n",*Turntable_speed);
    pc.printf("Time360 = %d, Match_wid =  %d\r\n", *Time360, *Match_wid);
    pc.printf("Camera_board_wait = %d\r\n", *Camera_board_wait);
    
    return SDerrorcount;
}            
int GetParameter(FILE *fp, const char *paramName,char parameter[]){
    int i=0, j=0;
    int strmax = 200;
    char str[strmax];

    rewind(fp); //ファイル位置を先頭に
    while(1){
        if (fgets(str, strmax, fp) == NULL) {
            return 0;
        }
        if (!strncmp(str, paramName, strlen(paramName))) {
            while (str[i++] != '=') {}
            while (str[i] != '\n') {
                    parameter[j++] = str[i++];
            }
            parameter[j] = '\0';
            return 1;
        }
    }
}  


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