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