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