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