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