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