main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050_DMP6.h"
00003 #include "HMC5883L.h"
00004 #include "SDFileSystem.h"
00005 #include "SkipperSv2.h"
00006 #include "falfalla.h"
00007 
00008 //MPU_check用
00009 #define PI 3.14159265358979
00010 
00011 #define servo_NEUTRAL_R    1614
00012 #define servo_NEUTRAL_L    1621
00013 #define servo_slow_FORWARD_R    1814
00014 #define servo_slow_FORWARD_L    1821
00015 #define servo_high_FORWARD_R    1714
00016 #define servo_high_FORWARD_L    1721
00017 #define turntable_NEUTRAL     1431 //カメラ台座のサーボ
00018 #define matchspeed 1714 //カメラと方向を合わせるときの車輪の速度
00019 #define focus_NEUTRAL  1455  //焦点合わせ用サーボ
00020 #define camera_deg_A 1400   //カメラ角度調整
00021 #define camera_deg_B 1800
00022 #define pint_speed 25
00023 #define pint_wait 3.5
00024 
00025 #define MOVE_NEUTRAL    0
00026 #define MOVE_FORWARD    1   
00027 #define MOVE_LEFT       2
00028 #define MOVE_RIGHT      3
00029 #define MOVE_BACK       4
00030 #define GOAL_FORWARD    5          //ゴール付近_ゆっくり
00031 #define GOAL_LEFT       6
00032 #define GOAL_RIGHT      7
00033 #define MAX_FORWARD     8          //はやい_姿勢修正用
00034 #define MAX_BACK        9
00035 
00036 void getSF_Serial_jevois();
00037 void getSF_Serial_pi();
00038 
00039 //MPU_check用
00040 void SensingMPU();
00041 
00042 void MoveCansat(char g_landingcommand);
00043 void setup();
00044 void Init_sensors();
00045 void DisplayClock();
00046 void DebugPrint();
00047 void SensingHMC();
00048 void MoveCameraBoard();
00049 void MatchPosition();
00050 void FocusAdjust();
00051 
00052 //sd設定
00053 int GetParameter(FILE *fp, const char *paramName,char parameter[]);
00054 
00055 int SetOptions(int *Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
00056             int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
00057             int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
00058             int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
00059             int *Camera_deg_A, int *Camera_deg_B,
00060             int *Pint_speed, float *Pint_wait
00061             );
00062 
00063 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
00064 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
00065 
00066 bool setupFlag = false;
00067 bool CameraDegFlag = false;
00068 bool jevoisFlag = true;
00069 
00070 char g_filename[30]="Datalog01.txt";
00071 
00072 enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度
00073 
00074 Timer t;
00075 
00076 FILE *g_fp;
00077 
00078 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
00079 
00080 RawSerial pc(PA_2,PA_3,115200); //uart2
00081 //pa2:UART2_TX,pa3:UART2_RX
00082 RawSerial pc2(PA_11,PA_12,115200); //uart1
00083 //pb6:UART1_TX,pb7:UART1_RX
00084 
00085 //PWM pin宣言
00086 PwmOut servoR(PC_6);           //TIM3_CH1 車輪右
00087 PwmOut servoL(PC_7);           //TIM3_CH2 車輪左
00088 PwmOut servoTurnTable(PB_0);   //TIM3_CH3 カメラ台回転Servo
00089 PwmOut servoCameradeg(PB_1);   //TIM3_CH4 カメラ角度調節Servo
00090 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
00091 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
00092 PwmOut servoGetUP(PC_8);       //skipperウラ 起き上がり動作
00093 
00094 /*通信用のpinは
00095   PA_3(UART2_Rx)   skipperウラ
00096   PA_12(UART6_Rx)  skipperオモテ USB端子より
00097   PB_7 (UART1_Rx)  skipperオモテ 6番目 TIM4_CH2
00098 */
00099 
00100 /*超音波はRaspberryPiに積む*/
00101 
00102 //外付けコンパス
00103 HMC5883L compass(PB_9, PB_8);    //コンパスセンサー TIM4_CH3とCH4
00104 
00105 char g_landingcommand='N';
00106 
00107 //MPU_check用
00108 MPU6050DMP6 mpu6050(PC_0,&pc);
00109 
00110 
00111 int main() {
00112     
00113     //MPU_check
00114     setup();
00115     
00116     servoCameradeg.pulsewidth_us(camera_deg_A);
00117     
00118     // シリアル通信受信の割り込みイベント登録
00119     pc.attach(getSF_Serial_jevois, Serial::RxIrq);
00120     pc2.attach(getSF_Serial_pi, Serial::RxIrq);
00121     
00122     while(1) {
00123         if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
00124         else NVIC_DisableIRQ(USART2_IRQn);
00125         
00126         MoveCameraBoard();
00127         
00128         if(g_landingcommand!='N') MatchPosition();
00129         
00130         if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
00131         else NVIC_DisableIRQ(USART2_IRQn);
00132         
00133         MoveCansat(g_landingcommand); 
00134         wait_ms(23);
00135         
00136         if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
00137         else NVIC_EnableIRQ(USART2_IRQn);
00138         }
00139 }
00140 
00141 
00142 void MoveCansat(char g_landingcommand)
00143 {
00144     //NVIC_DisableIRQ(USART1_IRQn);
00145     //NVIC_DisableIRQ(USART2_IRQn);
00146     switch(g_landingcommand){
00147         case 'N': //MOVE_NEUTRAL 
00148             servoR.pulsewidth_us(servo_NEUTRAL_R);
00149             servoL.pulsewidth_us(servo_NEUTRAL_L);
00150             //NVIC_EnableIRQ(USART1_IRQn);
00151             //NVIC_EnableIRQ(USART2_IRQn);
00152             break;
00153             
00154         case 'Y': //MOVE_FORWARD
00155             servoR.pulsewidth_us(servo_high_FORWARD_R);
00156             servoL.pulsewidth_us(servo_high_FORWARD_L);
00157             //NVIC_EnableIRQ(USART1_IRQn);
00158             //NVIC_EnableIRQ(USART2_IRQn); 
00159             break;
00160             
00161         case 'l': //MOVE_LEFT Low Speed
00162             servoR.pulsewidth_us(servo_slow_FORWARD_R);
00163             //NVIC_EnableIRQ(USART1_IRQn);
00164             //NVIC_EnableIRQ(USART2_IRQn); 
00165         
00166         case 'L': //MOVE_LEFT High Speed
00167             servoR.pulsewidth_us(servo_high_FORWARD_R);
00168             //NVIC_EnableIRQ(USART1_IRQn);
00169             //NVIC_EnableIRQ(USART2_IRQn); 
00170             
00171         case 'r': //MOVE_RIGHT Low Speed  
00172             servoL.pulsewidth_us(servo_slow_FORWARD_L);
00173             //NVIC_EnableIRQ(USART1_IRQn);
00174             //NVIC_EnableIRQ(USART2_IRQn); 
00175             break;
00176         
00177         case 'R': //MOVE_RIGHT  High Speed
00178             servoL.pulsewidth_us(servo_high_FORWARD_L);
00179             //NVIC_EnableIRQ(USART1_IRQn);
00180             //NVIC_EnableIRQ(USART2_IRQn); 
00181             break;
00182             
00183         case 'G': //GOAL_FORWARD
00184             servoR.pulsewidth_us(servo_slow_FORWARD_R);
00185             servoL.pulsewidth_us(servo_slow_FORWARD_L);
00186             //NVIC_EnableIRQ(USART1_IRQn);
00187             //NVIC_EnableIRQ(USART2_IRQn); 
00188             break;
00189         
00190         case '1': //MOVE_FORWARD Speed Level 1
00191             servoR.pulsewidth_us(servo_high_FORWARD_R);
00192             servoL.pulsewidth_us(servo_high_FORWARD_L);
00193             //NVIC_EnableIRQ(USART1_IRQn);
00194             wait(1);
00195             do{
00196             }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
00197             //NVIC_EnableIRQ(USART2_IRQn); 
00198             break;
00199         
00200         case '2': //MOVE_FORWARD Speed Level 2
00201             servoR.pulsewidth_us(servo_high_FORWARD_R);
00202             servoL.pulsewidth_us(servo_high_FORWARD_L);
00203             //NVIC_EnableIRQ(USART1_IRQn);
00204             wait(2);
00205             do{
00206             }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
00207             //NVIC_EnableIRQ(USART2_IRQn); 
00208             break;
00209         
00210         case '3': //MOVE_FORWARD Speed Level 3
00211             servoR.pulsewidth_us(servo_high_FORWARD_R);
00212             servoL.pulsewidth_us(servo_high_FORWARD_L);
00213             //NVIC_EnableIRQ(USART1_IRQn);
00214             wait(3);
00215             do{
00216             }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
00217             //NVIC_EnableIRQ(USART2_IRQn); 
00218             break;
00219         
00220         case '4': //MOVE_FORWARD Speed Level 4
00221             servoR.pulsewidth_us(servo_high_FORWARD_R);
00222             servoL.pulsewidth_us(servo_high_FORWARD_L);
00223             //NVIC_EnableIRQ(USART1_IRQn);
00224             wait(4);
00225             do{
00226             }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
00227             //NVIC_EnableIRQ(USART2_IRQn); 
00228             break;
00229             
00230         case '5': //MOVE_FORWARD Speed Level 5
00231             servoR.pulsewidth_us(servo_high_FORWARD_R);
00232             servoL.pulsewidth_us(servo_high_FORWARD_L);
00233             //NVIC_EnableIRQ(USART1_IRQn);
00234             wait(5);
00235             do{
00236             }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
00237             //NVIC_EnableIRQ(USART2_IRQn); 
00238             break;
00239             
00240         case 'M': //MatchPosition
00241             servoR.pulsewidth_us(matchspeed);
00242             //NVIC_EnableIRQ(USART1_IRQn);
00243             //NVIC_EnableIRQ(USART2_IRQn); 
00244             break;
00245         
00246         case 'P': //jevoisからraspberry piへの切り替え
00247             jevoisFlag = false;
00248             //NVIC_EnableIRQ(USART2_IRQn); 
00249             break;
00250         
00251         case 'S':
00252             /*RasPiからの超音波判定(プログラムスタート部)*/
00253             break;
00254             
00255         default :
00256             //NVIC_EnableIRQ(USART1_IRQn);
00257             //NVIC_EnableIRQ(USART2_IRQn);  
00258             break;
00259      
00260     }
00261     
00262     return;
00263 }
00264 
00265 void MoveCameraBoard(){
00266     MoveCansat('N');
00267     g_landingcommand='N';
00268     servoTurnTable.pulsewidth_us(1800);
00269     wait_ms(600);
00270     servoTurnTable.pulsewidth_us(turntable_NEUTRAL);
00271     if(jevoisFlag == true) FocusAdjust();
00272     else wait(1);
00273     
00274     if(g_landingcommand!='N') return;
00275     if(!CameraDegFlag){
00276         for(int i=0; i<camera_deg_B-camera_deg_A;i=i+10){
00277             servoCameradeg.pulsewidth_us(camera_deg_A+10);
00278             wait_ms(20);
00279             }
00280         CameraDegFlag=!CameraDegFlag;
00281         }else{
00282             for(int i=0; i<camera_deg_B-camera_deg_A;i=i+10){
00283             servoCameradeg.pulsewidth_us(camera_deg_A-10);
00284             }
00285         CameraDegFlag = !CameraDegFlag;
00286     }
00287         
00288     if(jevoisFlag == true) FocusAdjust();
00289     else wait(1);
00290     
00291     return;
00292 }
00293 
00294 void MatchPosition(){
00295     SensingMPU();
00296     SensingHMC();
00297     DebugPrint();
00298     
00299     while(nowAngle[YAW] <= nowAngle_HMC+5 && nowAngle[YAW] >= nowAngle_HMC-5){
00300         MoveCansat('M');
00301         SensingMPU();
00302     }
00303     return;
00304 }
00305 
00306 void FocusAdjust(){
00307     servoCameraPinto.pulsewidth_us(focus_NEUTRAL-200);
00308     //pc.printf("set\r\n");
00309     wait(1);
00310     
00311     servoCameraPinto.pulsewidth_us(focus_NEUTRAL+pint_speed);
00312     //pc.printf("check\r\n");
00313     wait(pint_wait);
00314     servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
00315       
00316   return;      
00317 }     
00318 
00319 void getSF_Serial_jevois(){
00320 
00321 
00322     static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
00323     
00324     static int bufcounter=0;
00325         
00326        
00327 
00328     if(pc.readable()) {    // 受信確認
00329         
00330         SFbuf[bufcounter] = pc.getc();    // 1文字取り出し
00331         if(SFbuf[0]!='S'){
00332              //pc.printf("x");
00333              return;
00334         }  
00335         
00336                 
00337         
00338         //pc.printf("%c",SFbuf[bufcounter]);
00339         
00340         if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
00341             
00342         if(bufcounter==5 && SFbuf[4]=='F'){
00343                 
00344             g_landingcommand = SFbuf[1];
00345             wait_ms(31);//信号が速すぎることによる割り込み防止
00346             //pc.printf("%c",g_landingcommand);
00347             //wait_ms(20);
00348             //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
00349             bufcounter = 0;
00350             memset(SFbuf, 0, sizeof(SFbuf));
00351             NVIC_ClearPendingIRQ(USART2_IRQn);
00352             //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
00353         }
00354             
00355         else if(bufcounter>=5){
00356             //pc.printf("Communication Falsed.\r\n");
00357             memset(SFbuf, 0, sizeof(SFbuf));
00358             bufcounter = 0;
00359             NVIC_ClearPendingIRQ(USART2_IRQn);
00360         }
00361     }
00362                     
00363 
00364 }
00365 
00366 
00367 void getSF_Serial_pi(){
00368     
00369     //NVIC_DisableIRQ(USART2_IRQn);
00370 
00371     static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
00372     
00373     static int bufcounter=0;
00374         
00375        
00376 
00377     if(pc2.readable()) {    // 受信確認
00378         
00379         SFbuf[bufcounter] = pc2.getc();    // 1文字取り出し
00380         if(SFbuf[0]!='S'){
00381              //pc.printf("x");
00382              return;
00383         }  
00384         
00385                 
00386         
00387         //pc.printf("%c",SFbuf[bufcounter]);
00388         
00389         if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
00390             
00391         if(bufcounter==5 && SFbuf[4]=='F'){
00392                 
00393             g_landingcommand = SFbuf[1];
00394             wait_ms(31);//信号が速すぎることによる割り込み防止
00395             //pc.printf("%c",g_landingcommand);
00396             //wait_ms(20);
00397             //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
00398             bufcounter = 0;
00399             memset(SFbuf, 0, sizeof(SFbuf));
00400             NVIC_ClearPendingIRQ(USART2_IRQn);
00401             //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
00402         }
00403             
00404         else if(bufcounter>=5){
00405             //pc.printf("Communication Falsed.\r\n");
00406             memset(SFbuf, 0, sizeof(SFbuf));
00407             bufcounter = 0;
00408             NVIC_ClearPendingIRQ(USART2_IRQn);
00409         }
00410     }
00411     
00412     //NVIC_EnableIRQ(USART2_IRQn);
00413                     
00414 }
00415 
00416 
00417 void setup(){
00418      
00419     SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
00420                &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
00421                &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
00422                &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
00423                &Camera_deg_A, &Camera_deg_B,
00424                &Pint_speed,&Pint_wait
00425                
00426                );
00427                
00428    g_fp = fopen( "/sd/Datalog01.txt" ,"r+" );
00429   if( g_fp == NULL ) {
00430     pc.printf( "ファイルオープンエラー\r\n" );
00431     }
00432    fprintf(g_fp,"Fall Start.\r\n");
00433    
00434    fclose( g_fp );
00435      
00436     Init_sensors();
00437     //switch2.rise(ResetTrim);
00438     
00439     //NVIC_SetPriority(USART1_IRQn,0);
00440     //NVIC_SetPriority(EXTI0_IRQn,1);
00441     //NVIC_SetPriority(TIM5_IRQn,2);
00442     //NVIC_SetPriority(EXTI9_5_IRQn,3);
00443     //NVIC_SetPriority(USART2_IRQn,4);
00444     
00445     NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
00446     NVIC_SetPriority(USART2_IRQn,1);
00447     
00448     DisplayClock();
00449     t.start();
00450     
00451     pc.printf("MPU calibration start\r\n");
00452     pc.printf("HMC calibration start\r\n");
00453     
00454     float offsetstart = t.read();
00455     while(t.read() - offsetstart < 26){
00456         SensingMPU();
00457         SensingHMC();
00458         for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
00459         pc.printf("%3.2f\t",nowAngle_HMC);
00460         pc.printf("\r\n");
00461     }
00462     
00463     FirstROLL = nowAngle[ROLL];
00464     FirstPITCH = nowAngle[PITCH];
00465     nowAngle[ROLL] -=FirstROLL;
00466     nowAngle[PITCH] -=FirstPITCH;
00467     FirstYAW = nowAngle[YAW];
00468     nowAngle[YAW] -= FirstYAW;
00469     
00470     g_FirstYAW_HMC = nowAngle_HMC;
00471     nowAngle_HMC -=g_FirstYAW_HMC;
00472     if(nowAngle_HMC<0)nowAngle_HMC+=360;
00473     
00474     wait(0.2);
00475     
00476     pc.printf("All initialized\r\n");
00477     setupFlag=true;
00478 }
00479 
00480 
00481 void SensingMPU(){
00482     //static int16_t deltaT = 0, t_start = 0;
00483     //t_start = t.read_us();
00484     
00485     float rpy[3] = {0};
00486     static uint16_t count_changeRPY = 0;
00487     static bool flg_checkoutlier = false;
00488     NVIC_DisableIRQ(USART1_IRQn);
00489     NVIC_DisableIRQ(USART2_IRQn);
00490     NVIC_DisableIRQ(TIM5_IRQn);
00491     NVIC_DisableIRQ(EXTI0_IRQn);
00492     NVIC_DisableIRQ(EXTI9_5_IRQn);
00493 
00494     mpu6050.getRollPitchYaw_Skipper(rpy);
00495  
00496     NVIC_EnableIRQ(USART1_IRQn);
00497     NVIC_EnableIRQ(USART2_IRQn);
00498     NVIC_EnableIRQ(TIM5_IRQn);
00499     NVIC_EnableIRQ(EXTI0_IRQn);
00500     NVIC_EnableIRQ(EXTI9_5_IRQn);
00501     
00502     
00503     //外れ値対策
00504     for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
00505     rpy[ROLL] -= FirstROLL;
00506     rpy[PITCH] -= FirstPITCH;
00507     if(!setupFlag){
00508         rpy[YAW] -= FirstYAW;
00509         }else{
00510         if(rpy[YAW] >= FirstYAW){
00511             rpy[YAW] -= FirstYAW;
00512             }else{
00513             rpy[YAW] += 360.0f;
00514             rpy[YAW] -= FirstYAW;
00515         }
00516     }
00517     
00518     for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
00519     if(!flg_checkoutlier || count_changeRPY >= 2){
00520         for(uint8_t i=0; i<3; i++){
00521             nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
00522         }
00523         count_changeRPY = 0;
00524     }else   count_changeRPY++;
00525     flg_checkoutlier = false;
00526     
00527 }
00528 
00529 
00530 void Init_sensors(){
00531     if(mpu6050.setup() == -1){
00532         pc.printf("failed initialize\r\n");
00533     }
00534 }
00535 
00536 
00537 void DisplayClock(){
00538     pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
00539     pc.printf("HCLK Clock   = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
00540     pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
00541     pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
00542     pc.printf("\r\n");
00543 }
00544     
00545 void SensingHMC(){
00546     //static int16_t deltaT = 0, t_start = 0;
00547     //t_start = t.read_us();
00548     
00549     float rpy=0;
00550     static uint16_t count_changeRPY = 0;
00551     static bool flg_checkoutlier = false;
00552     NVIC_DisableIRQ(USART1_IRQn);
00553     NVIC_DisableIRQ(USART2_IRQn);
00554     NVIC_DisableIRQ(TIM5_IRQn);
00555     NVIC_DisableIRQ(EXTI0_IRQn);
00556     NVIC_DisableIRQ(EXTI9_5_IRQn);
00557 
00558     rpy= compass.getHeadingXYDeg(20,50);
00559  
00560     NVIC_EnableIRQ(USART1_IRQn);
00561     NVIC_EnableIRQ(USART2_IRQn);
00562     NVIC_EnableIRQ(TIM5_IRQn);
00563     NVIC_EnableIRQ(EXTI0_IRQn);
00564     NVIC_EnableIRQ(EXTI9_5_IRQn);
00565     
00566     
00567     //外れ値対策
00568     //rpy*= 180.0f/PI;
00569     if(!setupFlag){
00570         rpy -= g_FirstYAW_HMC;
00571         }else{
00572         if(rpy >= g_FirstYAW_HMC){
00573             rpy -= g_FirstYAW_HMC;
00574             }else{
00575             rpy += 360.0f;
00576             rpy -= g_FirstYAW_HMC;
00577         }
00578     }
00579     
00580     if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
00581     if(!flg_checkoutlier || count_changeRPY >= 2){
00582         
00583         nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f;  //2つの移動平均
00584         
00585         count_changeRPY = 0;
00586     }else   count_changeRPY++;
00587     flg_checkoutlier = false;
00588     
00589 }
00590 
00591 int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
00592                int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
00593                int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
00594                int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
00595                int *Camera_deg_A, int *Camera_deg_B,
00596                int *Pint_speed, float *Pint_wait
00597                ){
00598 
00599     pc.printf("SDsetup start.\r\n");    
00600     
00601     FILE *fp;
00602     char parameter[20]; //文字列渡す用の配列
00603     int SDerrorcount = 0;  //取得できなかった数を返す
00604     const char *paramNames[] = { 
00605         "SERVO_NEUTRAL_R",
00606         "SERVO_NEUTRAL_L",
00607         "SERVO_HIGH_FORWARD_R",
00608         "SERVO_HIGH_FORWARD_L",
00609         "SERVO_SLOW_FORWARD_R",
00610         "SERVO_SLOW_FORWARD_L",
00611         "TURNTABLE_NEUTRAL",
00612         "MATCH_SPEED",
00613         "FOCUS_NEUTRAL",
00614         "CAMERA_DEG_A",
00615         "CAMERA_DEG_B",
00616         "PINT_SPEED",
00617         "PINT_WAIT"
00618     };
00619 
00620     fp = fopen("/sd/option.txt","r");
00621     
00622     if(fp != NULL){ //開けたら
00623         pc.printf("File was openned.\r\n");
00624         if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
00625         else{                                        *Servo_NEUTRAL_R = servo_NEUTRAL_R;
00626                                                      SDerrorcount++;
00627         }
00628         if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
00629         else{                                        *Servo_NEUTRAL_L = servo_NEUTRAL_L;
00630                                                       SDerrorcount++;
00631         }
00632         if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter);
00633         else{                                        *Servo_high_FORWARD_R = servo_high_FORWARD_R;
00634                                                      SDerrorcount++;
00635         }
00636         if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
00637         else{                                        *Servo_high_FORWARD_L = servo_high_FORWARD_L;
00638                                                       SDerrorcount++;
00639         }
00640         if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
00641         else{                                        *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
00642                                                       SDerrorcount++;
00643         }
00644         if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
00645         else{                                        *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
00646                                                       SDerrorcount++;
00647         }
00648         
00649         if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
00650         else{                                         *Turntable_NEUTRAL = turntable_NEUTRAL;
00651                                                       SDerrorcount++;
00652         }
00653         if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
00654         else{                                         *Matchspeed = matchspeed;
00655                                                       SDerrorcount++;
00656         }
00657         if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
00658         else{                                         *Focus_NEUTRAL = focus_NEUTRAL;
00659                                                       SDerrorcount++;
00660         }
00661          if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
00662         else{                                         *Camera_deg_A = camera_deg_A;
00663                                                       SDerrorcount++;
00664         }
00665          if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
00666         else{                                         *Camera_deg_B = camera_deg_B;
00667                                                       SDerrorcount++;
00668         }
00669          if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
00670         else{                                         *Pint_speed = pint_speed;
00671                                                       SDerrorcount++;
00672         }
00673          if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
00674         else{                                         *Pint_wait = pint_wait;
00675                                                       SDerrorcount++;
00676         }
00677         
00678         fclose(fp);
00679 
00680     }else{  //ファイルがなかったら
00681         pc.printf("fp was null.\r\n");
00682         
00683         *Servo_NEUTRAL_R = servo_NEUTRAL_R;
00684         *Servo_NEUTRAL_L = servo_NEUTRAL_L;
00685         *Servo_high_FORWARD_R = servo_high_FORWARD_R;
00686         *Servo_high_FORWARD_L = servo_high_FORWARD_L;
00687         *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
00688         *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
00689         *Turntable_NEUTRAL = turntable_NEUTRAL;
00690         *Matchspeed = matchspeed;
00691         *Focus_NEUTRAL = focus_NEUTRAL;
00692         *Camera_deg_A = camera_deg_A;
00693         *Camera_deg_B = camera_deg_B;
00694         *Pint_speed = pint_speed;
00695         *Pint_wait = pint_wait;
00696         SDerrorcount = -1;
00697     }
00698     pc.printf("SDsetup finished.\r\n");
00699     if(SDerrorcount == 0)  pc.printf("setting option is success\r\n");
00700     else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
00701     else if(SDerrorcount > 0)  pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 
00702     
00703     pc.printf("Servo_NEUTRAL_R = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
00704     pc.printf("Servo_high_FORWARD_R = %d, Servo_high_FORWARD_L = %d\r\n", *Servo_high_FORWARD_R, *Servo_high_FORWARD_L);
00705     pc.printf("Servo_slow_FORWARD_R = %d, Servo_slow_FORWARD_L = %d\r\n", *Servo_slow_FORWARD_R, *Servo_slow_FORWARD_L);
00706     pc.printf("Turntable_NEUTRAL = %d, Matchspeed =  %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
00707     pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
00708     pc.printf("Camera_deg_A = %d, Camera_deg_B =  %d\r\n", *Camera_deg_A, *Camera_deg_B);
00709     pc.printf("Pint_speed = %d, Pint_wait =  %f\r\n", *Pint_speed, *Pint_wait);
00710     
00711     return SDerrorcount;
00712 }            
00713 
00714 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
00715     int i=0, j=0;
00716     int strmax = 200;
00717     char str[strmax];
00718 
00719     rewind(fp); //ファイル位置を先頭に
00720     while(1){
00721         if (fgets(str, strmax, fp) == NULL) {
00722             return 0;
00723         }
00724         if (!strncmp(str, paramName, strlen(paramName))) {
00725             while (str[i++] != '=') {}
00726             while (str[i] != '\n') {
00727                     parameter[j++] = str[i++];
00728             }
00729             parameter[j] = '\0';
00730             return 1;
00731         }
00732     }
00733 }  
00734 
00735 
00736     
00737 void DebugPrint(){
00738     //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
00739     //pc.printf("%3.2f\t",nowAngle[2]);
00740     //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
00741     //pc.printf("\r\n");
00742     }