Bot Furukawa
/
Estrela_v12
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.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 //#include "rtos.h" 00003 #include "MPU9250.h" 00004 #include "BMP280.h" 00005 #include "SkipperSv2.h" 00006 #include "Estrela.h" 00007 //#include "stm32f4xx_hal_iwdg.h" 00008 00009 00010 #define UPD_MANUAL 0 00011 #define UPD_AUTO 1 00012 #define UPD_LANDING 2 00013 00014 #define SBUS_SIGNAL_OK 0x00 00015 #define SBUS_SIGNAL_LOST 0x01 00016 #define SBUS_SIGNAL_FAILSAFE 0x03 00017 00018 //Serial pc(TX, RX); 00019 RawSerial sbus_(PA_9, PA_10); //SBUS 00020 00021 PwmOut servo1(PC_6); // TIM3_CH1 00022 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 00023 PwmOut servo3(PB_0); // TIM3_CH3 00024 PwmOut servo4(PB_1); // TIM3_CH4 00025 PwmOut servo5(PB_6); // TIM4_CH1 00026 PwmOut servo6(PB_7); // TIM4_CH2 00027 PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 00028 PwmOut servo8(PB_9); // TIM4_CH4 00029 00030 RawSerial pc(PA_2, PA_3); 00031 00032 DigitalOut led1(PA_0); //緑 00033 DigitalOut led2(PA_1); //赤 00034 DigitalOut led3(PB_4); //赤外線 00035 DigitalOut led4(PB_5); 00036 00037 volatile uint8_t buf_sbus[25]; 00038 volatile int cnt_sbus = 0; 00039 static int16_t channels[18]; 00040 uint8_t failsafe_status = SBUS_SIGNAL_FAILSAFE; 00041 static bool flg_ch_update = false; 00042 //uint8_t i,j,k; 00043 uint8_t update_mode = 0; 00044 static uint16_t pwm[8]; 00045 static uint16_t oldTHL; 00046 00047 //9軸センサー関連 00048 float sum = 0; 00049 uint32_t sumCount = 0; 00050 MPU9250 mpu9250; 00051 Timer t; 00052 00053 /* 00054 float g_BefGyro[3] = {0,0,0}; 00055 double g_HpfGyro[3] = {0,0,0}; 00056 double g_HpfGyro1[3] = {0,0,0}; 00057 double g_LpfAccel[3] = {0,0,0}; 00058 */ 00059 00060 static int jj = 0; 00061 00062 //IWDG_HandleTypeDef hiwdg; 00063 00064 void disp_clock(){ 00065 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); 00066 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); 00067 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); 00068 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); 00069 pc.printf("\r\n"); 00070 } 00071 00072 //サーボ初期化関数 00073 void init_Servo() 00074 { 00075 uint8_t j = 0; 00076 00077 servo1.period_ms(14); 00078 servo1.pulsewidth_us(1500); 00079 00080 servo2.period_ms(14); 00081 servo2.pulsewidth_us(1500); 00082 00083 servo3.period_ms(14); 00084 servo3.pulsewidth_us(1000); 00085 00086 servo4.period_ms(14); 00087 servo4.pulsewidth_us(1500); 00088 00089 servo5.period_ms(14); 00090 servo5.pulsewidth_us(1500); 00091 00092 servo6.period_ms(14); 00093 servo6.pulsewidth_us(1500); 00094 00095 servo7.period_ms(14); 00096 servo7.pulsewidth_us(1500); 00097 00098 servo8.period_ms(14); 00099 servo8.pulsewidth_us(1500); 00100 00101 for (j=0; j<8; j++) {pwm[j] = 1500;} 00102 pc.printf("servo initialized\r\n"); 00103 } 00104 00105 //---setall_servo--- 00106 //pwmをサーボに出力する関数。 00107 void setall_servo() 00108 { 00109 uint8_t j = 0; 00110 00111 //if(failsafe_status == SBUS_SIGNAL_OK){ 00112 servo1.pulsewidth_us(pwm[0]); 00113 servo2.pulsewidth_us(pwm[1]); 00114 servo3.pulsewidth_us(pwm[2]); 00115 servo4.pulsewidth_us(pwm[3]); 00116 servo5.pulsewidth_us(pwm[4]); 00117 servo6.pulsewidth_us(pwm[5]); 00118 servo7.pulsewidth_us(pwm[6]); 00119 servo8.pulsewidth_us(pwm[7]); 00120 /* }else{ 00121 servo1.pulsewidth_us(1000); 00122 servo2.pulsewidth_us(1000); 00123 servo3.pulsewidth_us(1000); 00124 servo4.pulsewidth_us(1000); 00125 servo5.pulsewidth_us(1000); 00126 servo6.pulsewidth_us(1000); 00127 servo7.pulsewidth_us(1000); 00128 servo8.pulsewidth_us(1000); 00129 }*/ 00130 00131 for(j=0;j<8;j++){ 00132 pc.printf("ch%d=%d ", j+1, pwm[j]); 00133 } 00134 //pc.printf("time = %d", frq.read_ms()); 00135 pc.printf("\r\n"); 00136 00137 //frq.reset(); 00138 00139 } 00140 00141 00142 //---update_pwm--- 00143 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) 00144 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) 00145 void update_pwm() 00146 { 00147 uint8_t j = 0; 00148 00149 if(flg_ch_update == true){ 00150 switch(update_mode){ //マニュアルモード,自動モード,自動着陸もモードを切替 00151 case UPD_MANUAL: //マニュアルモード 00152 for(j=0;j<8;j++){ 00153 pwm[j] = (channels[j]>>1) + 1000; 00154 } 00155 oldTHL = pwm[2]; 00156 jj=0; 00157 //pc.printf("update_manual\r\n"); 00158 break; 00159 00160 case UPD_AUTO: //自動モード 00161 pwm[0] = (channels[0]>>1) + 1000; 00162 pwm[1] = Auto_ELE; 00163 pwm[2] = Auto_THR; 00164 pwm[3] = Auto_RUD; 00165 pwm[4] = (channels[4]>>1) + 1000; 00166 pwm[5] = (channels[5]>>1) + 1000; 00167 pwm[6] = (channels[6]>>1) + 1000; 00168 pwm[7] = (channels[7]>>1) + 1000; 00169 00170 //pc.printf("update_auto\r\n"); 00171 break; 00172 00173 case UPD_LANDING: //自動着陸モード 00174 pwm[0] = (channels[0]>>1) + 1000; 00175 pwm[1] = Auto_ELE; 00176 pwm[2] = (channels[2]>>1) + 1000; 00177 pwm[3] = (channels[3]>>1) + 1000; 00178 pwm[4] = (channels[4]>>1) + 1000; 00179 pwm[5] = (channels[5]>>1) + 1000; 00180 pwm[6] = (channels[6]>>1) + 1000; 00181 pwm[7] = (channels[7]>>1) + 1000; 00182 break; 00183 00184 default: //デフォはマニュアルモード 00185 for(j=0;j<8;j++){ 00186 pwm[j] = (channels[j]>>1) + 1000; 00187 } 00188 00189 jj=0; 00190 //pc.printf("update_manual\r\n"); 00191 break; 00192 } 00193 }else{ 00194 pc.printf("0\r\n"); 00195 } 00196 flg_ch_update = false; 00197 ///oldTHL = pwm[2]; 00198 setall_servo(); 00199 } 00200 00201 /*void update_auto() 00202 { 00203 pwm[0] = (channels[0]>>1) + 1000; 00204 pwm[1] = Auto_ELE; 00205 pwm[2] = Auto_THR; 00206 pwm[3] = Auto_RUD; 00207 pwm[4] = (channels[4]>>1) + 1000; 00208 pwm[5] = (channels[5]>>1) + 1000; 00209 pwm[6] = (channels[6]>>1) + 1000; 00210 pwm[7] = (channels[7]>>1) + 1000; 00211 00212 setall_servo(); 00213 //pc.printf("update_auto\r\n"); 00214 }*/ 00215 00216 //---update_Input--- 00217 //catch_sbusで取得したSbusの生データから各チャンネルのpwmを抽出する。 00218 void update_Input() 00219 { 00220 uint8_t i = 0; 00221 00222 /* for (i=0; i<25; i++){ 00223 pc.printf("%x ", buf_sbus[i]); 00224 }pc.printf("\n"); 00225 */ 00226 // clear channels[] 00227 for (i=0; i<18; i++) {channels[i] = 0;} 00228 00229 // reset counters 00230 uint8_t byte_in_sbus = 1; 00231 uint8_t bit_in_sbus = 0; 00232 uint8_t ch = 0; 00233 uint8_t bit_in_channel = 0; 00234 00235 // process actual sbus data 00236 for (i=0; i<176; i++) { 00237 if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) { 00238 channels[ch] |= (1<<bit_in_channel); 00239 } 00240 bit_in_sbus++; 00241 bit_in_channel++; 00242 00243 if (bit_in_sbus == 8) { 00244 bit_in_sbus =0; 00245 byte_in_sbus++; 00246 } 00247 if (bit_in_channel == 11) { 00248 bit_in_channel =0; 00249 ch++; 00250 } 00251 } 00252 // DigiChannel 1 00253 if (buf_sbus[23] & (1<<0)) { 00254 channels[16] = 1; 00255 }else{ 00256 channels[16] = 0; 00257 } 00258 // DigiChannel 2 00259 if (buf_sbus[23] & (1<<1)) { 00260 channels[17] = 1; 00261 }else{ 00262 channels[17] = 0; 00263 } 00264 // Failsafe 00265 failsafe_status = SBUS_SIGNAL_OK; 00266 if (buf_sbus[23] & (1<<2)) { 00267 failsafe_status = SBUS_SIGNAL_LOST; 00268 } 00269 if (buf_sbus[23] & (1<<3)) { 00270 failsafe_status = SBUS_SIGNAL_FAILSAFE; 00271 } 00272 //channels[i] = channels[i]>>1; 00273 for (i=0; i<25; i++) {buf_sbus[i] = 0;} 00274 cnt_sbus = 0; 00275 flg_ch_update = true; 00276 00277 update_pwm(); 00278 00279 00280 } 00281 00282 //---catch_sbus--- 00283 //受信機から送られてくるSBus信号(シリアル)を取得する関数 00284 void catch_sbus() 00285 { 00286 buf_sbus[cnt_sbus] = sbus_.getc(); 00287 if(buf_sbus[0] == 0x0f) 00288 { 00289 cnt_sbus ++; 00290 } 00291 if(cnt_sbus >=25){ 00292 if(buf_sbus[24] == 0x00){ 00293 update_Input(); //update_Input実行 00294 }else{ 00295 cnt_sbus = 0; 00296 } 00297 } 00298 return; 00299 } 00300 00301 //---init_sbus--- 00302 //Sbusを初期化する関数 00303 void init_sbus(){ 00304 // Set Baudrate 00305 sbus_.baud(100000); 00306 // Set Datalenght & Frame 00307 sbus_.format(8, Serial::Even, 2); 00308 //start sbus irq 00309 sbus_.attach(&catch_sbus, RawSerial::RxIrq); 00310 } 00311 00312 //---CheckSW_up--- 00313 //プロポのスイッチがonかoffか返す関数 00314 //ch 1~8 00315 bool CheckSW_up(int8_t ch) 00316 { 00317 uint16_t checkpwm; 00318 checkpwm = (channels[ch-1]>>1) + 1000; 00319 //pc.printf("ch%d=%d ", ch, checkpwm); 00320 if(SWITCH_CHECK > checkpwm){ 00321 return true; 00322 }else{ 00323 return false; 00324 } 00325 } 00326 00327 00328 00329 //---sensing--- 00330 //センサーの値を読み込み、各種フィルタをかける(認知) 00331 void sensing() 00332 { 00333 // If intPin goes high, all data registers have new data 00334 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt 00335 00336 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values 00337 // Now we'll calculate the accleration value into actual g's 00338 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 00339 ay = (float)accelCount[1]*aRes - accelBias[1]; 00340 az = (float)accelCount[2]*aRes - accelBias[2]; 00341 00342 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values 00343 // Calculate the gyro value into actual degrees per second 00344 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set 00345 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 00346 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 00347 00348 mpu9250.readMagData(magCount); // Read the x/y/z adc values 00349 // Calculate the magnetometer values in milliGauss 00350 // Include factory calibration per data sheet and user environmental corrections 00351 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set 00352 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; 00353 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; 00354 } 00355 00356 Now = t.read_us(); 00357 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update 00358 lastUpdate = Now; 00359 00360 sum += deltat; 00361 sumCount++; 00362 00363 // if(lastUpdate - firstUpdate > 10000000.0f) { 00364 // beta = 0.04; // decrease filter gain after stabilized 00365 // zeta = 0.015; // increasey bias drift gain after stabilized 00366 // } 00367 00368 // Pass gyro rate as rad/s 00369 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00370 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 00371 00372 /* Auto_Loop()で50ms待ってるから要らないと思う 00373 // Serial print and/or display at 0.5 s rate independent of data rates 00374 delt_t = t.read_ms() - count; 00375 if (delt_t > 500) { // update LCD once per half-second independent of read rate 00376 */ 00377 00378 //pc.printf("ax = %f", 1000*ax); 00379 //pc.printf(" ay = %f", 1000*ay); 00380 //pc.printf(" az = %f mg\n\r", 1000*az); 00381 00382 //pc.printf("gx = %f", gx); 00383 //pc.printf(" gy = %f", gy); 00384 //pc.printf(" gz = %f deg/s\n\r", gz); 00385 00386 //pc.printf("mx = %f,", mx); 00387 //pc.printf(" my = %f,", my); 00388 //pc.printf(" mz = %f mG\n\r", mz); 00389 00390 tempCount = mpu9250.readTempData(); // Read the adc values 00391 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade 00392 //pc.printf(" temperature = %f C\n\r", temperature); 00393 00394 //pc.printf("q0 = %f\n\r", q[0]); 00395 //pc.printf("q1 = %f\n\r", q[1]); 00396 //pc.printf("q2 = %f\n\r", q[2]); 00397 //pc.printf("q3 = %f\n\r", q[3]); 00398 00399 00400 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 00401 // In this coordinate system, the positive z-axis is down toward Earth. 00402 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. 00403 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 00404 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 00405 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 00406 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 00407 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 00408 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 00409 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 00410 roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00411 pitch = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 00412 pitch *= 180.0f / PI; 00413 yaw *= 180.0f / PI; 00414 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 00415 roll *= 180.0f / PI; 00416 00417 //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100)); 00418 //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll); 00419 //pc.printf("average rate = %f\n\r", (float) sumCount/sum); 00420 00421 count = t.read_ms(); 00422 sum = 0; 00423 sumCount = 0; 00424 //} 00425 } 00426 00427 00428 //---CalibrateCompass--- 00429 //グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない 00430 //使わない予定 00431 void CalibrateCompass(void) 00432 { 00433 led1 = 1; 00434 sensing(); 00435 uint8_t ii; 00436 float mxMAX=mx, mxMIN=mx; 00437 float myMAX=my, myMIN=my; 00438 magbias[0] = 0; //初期化 00439 magbias[1] = 0; 00440 //magbias[2] = 0; 00441 ii=0; 00442 while(1){ //キャリブレーション実行 led2が点滅時に機体を一回転させる xy方向のみ 00443 sensing(); 00444 if(mxMAX < mx) mxMAX = mx; 00445 if(mxMIN > mx) mxMIN = mx; 00446 if(myMAX < my) myMAX = my; 00447 if(myMIN > my) myMIN = my; 00448 led2 = !led2; 00449 ++ii; 00450 wait(50); 00451 00452 if(!CheckSW_up(7)) break; //スイッチ7を下げて終了 00453 } 00454 led2 = 0; 00455 if(ii<20){ //チャンネル7をすぐ切った場合 元に戻す 00456 magbias[0] = MAGBIASX; 00457 magbias[1] = MAGBIASY; 00458 }else{ //しっかりと値を入れた場合 00459 magbias[0] = (mxMAX+mxMIN)/2; 00460 magbias[1] = (myMAX+myMIN)/2; 00461 } 00462 led1 = 0; 00463 t.reset(); //タイマーをリセットして終了 00464 } 00465 00466 00467 //---StabiGyro--- 00468 //水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) 00469 //右旋回 00470 void StabiGyro() 00471 { 00472 if(0 /*oldTHL < 1200*/){ //機体審査用 00473 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN-gy*RUD_DGN); 00474 Auto_ELE = ELE_N + -50 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00475 Auto_THR = 1500; 00476 }else{ 00477 if(jj<=0){ //旋回し始め 15 00478 Auto_RUD = RUD_N + -65 + int((-12 -roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5 00479 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx 00480 Auto_THR = oldTHL + 30; 00481 //pc.printf("first%d\r\n",jj); 00482 jj++; 00483 }else{ //旋回中 00484 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); 00485 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00486 Auto_THR = oldTHL ; 00487 } 00488 } 00489 00490 //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする 00491 if(Auto_RUD > RUD_MAX){ 00492 Auto_RUD = RUD_MAX; 00493 }else if(Auto_RUD < RUD_MIN){ 00494 Auto_RUD = RUD_MIN; 00495 } 00496 if(Auto_ELE > ELE_MAX){ 00497 Auto_ELE = ELE_MAX; 00498 }else if(Auto_ELE < ELE_MIN){ 00499 Auto_ELE = ELE_MIN; 00500 } 00501 00502 00503 00504 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); 00505 //Thread::wait(G_MS); 00506 } 00507 00508 //---StabiGyro_Mobius--- 00509 //8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) 00510 void StabiGyro_Mobius() 00511 { 00512 if(jj<=15){ //右旋回導入 00513 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy + 130)*0.75); //*0.75); //roll -> gy 3.5 00514 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx + 30)*0.8); //pitch -> -gx 00515 Auto_THR = oldTHL; 00516 //pc.printf("first%d\r\n",jj); 00517 jj++; 00518 }else if((15<jj) && (jj<=70)){ //右旋回途中 00519 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); 00520 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00521 Auto_THR = oldTHL; 00522 jj++; 00523 }else if((70<jj) && (jj<=75)){ //旋回遷移1 00524 Auto_RUD = RUD_N + 100 + int((16 -roll)*RUDDER_GN - (gy-100)*0.75); 00525 Auto_ELE = ELE_N + 20 + int((0 -pitch)*ELEVATOR_GN - (gx + 0)*0.6); 00526 Auto_THR = THR_N - 100; 00527 //pc.printf("first%d\r\n",jj); 00528 jj++; 00529 }else if((75<jj) && (jj<=90)){ //旋回遷移2 00530 Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - (gy-100)*0.75); // 105 30 00531 Auto_ELE = ELE_N + -15 + int((-5 - pitch)*ELEVATOR_GN - (gx+30)*0.6); //-75 -15 00532 Auto_THR = oldTHL - 20; //-20 00533 jj++; 00534 }else{ //左旋回途中 00535 Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - gy*RUD_DGN); 00536 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00537 Auto_THR = oldTHL; 00538 } 00539 00540 if(Auto_RUD > RUD_MAX){ 00541 Auto_RUD = RUD_MAX; 00542 }else if(Auto_RUD < RUD_MIN){ 00543 Auto_RUD = RUD_MIN; 00544 } 00545 if(Auto_ELE > ELE_MAX){ 00546 Auto_ELE = ELE_MAX; 00547 }else if(Auto_ELE < ELE_MIN){ 00548 Auto_ELE = ELE_MIN; 00549 } 00550 00551 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); 00552 //Thread::wait(G_MS); 00553 } 00554 00555 //---StabiGyro_Climb--- 00556 //上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) 00557 void StabiGyro_Climb() 00558 { 00559 if(jj<=15){ //右旋回導入 00560 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy+130)*RUD_DGN); //*0.75); //roll -> gy 3.5 00561 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx+30)*ELE_DGN); //pitch -> -gx 00562 Auto_THR = oldTHL; 00563 //pc.printf("first%d\r\n",jj); 00564 jj++; 00565 }else if((15<jj) && (jj<=136)){ //右旋回途中 00566 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); 00567 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00568 Auto_THR = oldTHL; 00569 jj++; 00570 /*}else if((136<jj) && (jj<=146)){ //上昇遷移 00571 Auto_RUD = RUD_N + -65 - int((-12-roll)*RUDDER_GN - gy*0); 00572 Auto_ELE = ELE_N + -80 + int((-22-pitch)*ELEVATOR_GN - gx*0); 00573 Auto_THR = THR_N + 100; 00574 //pc.printf("first%d\r\n",jj); 00575 jj++;*/ 00576 }else if((136<jj) && (jj<=200)){ //上昇中 00577 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); 00578 Auto_ELE = ELE_N + -140 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00579 Auto_THR = oldTHL + 220; 00580 jj++; 00581 }else{ //水平旋回 00582 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); 00583 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00584 Auto_THR = oldTHL; 00585 } 00586 00587 if(Auto_RUD > RUD_MAX){ 00588 Auto_RUD = RUD_MAX; 00589 }else if(Auto_RUD < RUD_MIN){ 00590 Auto_RUD = RUD_MIN; 00591 } 00592 if(Auto_ELE > ELE_MAX){ 00593 Auto_ELE = ELE_MAX; 00594 }else if(Auto_ELE < ELE_MIN){ 00595 Auto_ELE = ELE_MIN; 00596 } 00597 00598 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); 00599 //Thread::wait(G_MS); 00600 } 00601 00602 //---StabiGyro_Glide--- 00603 //自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) 00604 void StabiGyro_Glide() 00605 { 00606 if(jj<=15){ 00607 Auto_RUD = RUD_N; 00608 Auto_ELE = ELE_N + -5 + int (( - pitch)*ELEVATOR_GN - gx*0); 00609 Auto_THR = 1000; 00610 //pc.printf("first%d\r\n",jj); 00611 jj++; 00612 }else{ 00613 Auto_RUD = RUD_N + -30 + int((-8 - roll)*RUDDER_GN - gy*RUD_DGN); 00614 Auto_ELE = ELE_N + -25 + int((-5- pitch)*ELEVATOR_GN - gx*ELE_DGN); 00615 Auto_THR = 1000; //スロットルoff 00616 } 00617 if(Auto_RUD > RUD_MAX){ 00618 Auto_RUD = RUD_MAX; 00619 }else if(Auto_RUD < RUD_MIN){ 00620 Auto_RUD = RUD_MIN; 00621 } 00622 if(Auto_ELE > ELE_MAX){ 00623 Auto_ELE = ELE_MAX; 00624 }else if(Auto_ELE < ELE_MIN){ 00625 Auto_ELE = ELE_MIN; 00626 } 00627 00628 //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); 00629 //Thread::wait(G_MS); 00630 } 00631 00632 void StabiGyro_Landing() 00633 { 00634 if(pwm[0]<1300){ 00635 Auto_ELE = ELE_N + -10 + int((-5 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00636 Auto_THR = oldTHL; 00637 }else if((1300<=pwm[0])&&(pwm[0]<1800)){ 00638 Auto_ELE = ELE_N + -15 + int((-8 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00639 Auto_THR = oldTHL - 30; 00640 }else if(1800<=pwm[0]){ 00641 Auto_ELE = ELE_N + -25 + int((-12 - pitch)*ELEVATOR_GN - gx*ELE_DGN); 00642 Auto_THR = 1000; 00643 } 00644 } 00645 00646 void Auto_Loop() 00647 { 00648 //while(true){ 00649 //for(k=0;k<100;++k){ 00650 //sensing(); //センサーの値を読み込む(認知) 00651 StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断) 00652 update_mode = UPD_AUTO; //オートモード 00653 //StabiAccel(); 00654 //Auto_ELE+=145; 00655 //Auto_RUD-=80; 00656 //Servos.Auto2Servo(); 00657 //Thread::wait(50); 00658 /* 00659 if(!CheckSW_up(7)){ 00660 pc.printf("go to manual\n"); 00661 break; 00662 }*/ 00663 //wait_ms(50); 00664 //} 00665 //k=0; //for debug 00666 //pwm[6]=1000; 00667 } 00668 00669 void Auto_Mobius() 00670 { 00671 //sensing(); 00672 StabiGyro_Mobius(); 00673 update_mode = UPD_AUTO; 00674 //wait_ms(50); 00675 } 00676 00677 void Auto_Climb() 00678 { 00679 //sensing(); 00680 StabiGyro_Climb(); 00681 update_mode = UPD_AUTO; 00682 //wait_ms(50); 00683 } 00684 00685 void Auto_Glide() 00686 { 00687 //sensing(); 00688 StabiGyro_Glide(); 00689 update_mode = UPD_AUTO; 00690 //wait_ms(50); 00691 } 00692 00693 void Auto_Landing() 00694 { 00695 //sensing(); 00696 StabiGyro_Landing(); 00697 update_mode = UPD_MANUAL; 00698 //wait_ms(50); 00699 } 00700 00701 int main() 00702 { 00703 //初期化開始 00704 led1 = 1; 00705 led2 = 1; 00706 led3 = 0; 00707 led4 = 0; 00708 00709 //SBus用バッファ初期化 00710 int count_initsbus; 00711 for (count_initsbus = 0; count_initsbus < 25; count_initsbus++) {buf_sbus[count_initsbus] = 0;} 00712 00713 //pc.baud(115200); //パソコン通信用シリアルのボードレート 00714 00715 //frq.start(); 00716 init_Servo(); 00717 init_sbus(); 00718 disp_clock(); 00719 00720 //---9軸センサー(MPU9250)初期化--- 00721 00722 //Set up I2C 00723 i2c.frequency(400000); // use fast (400 kHz) I2C 00724 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); 00725 t.start(); 00726 00727 // Read the WHO_AM_I register, this is a good test of communication 00728 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 00729 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r"); 00730 00731 if (whoami == 0x71){ // WHO_AM_I should always be 0x68 00732 pc.printf("MPU9250 is online...\n\r"); 00733 //wait(1); 00734 00735 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration 00736 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 00737 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]); 00738 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]); 00739 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]); 00740 //pc.printf("x accel bias = %f\n\r", accelBias[0]); 00741 //pc.printf("y accel bias = %f\n\r", accelBias[1]); 00742 //pc.printf("z accel bias = %f\n\r", accelBias[2]); 00743 wait(1); 00744 00745 mpu9250.initMPU9250(); 00746 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 00747 mpu9250.initAK8963(magCalibration); 00748 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer 00749 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); 00750 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); 00751 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r"); 00752 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r"); 00753 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r"); 00754 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r"); 00755 wait(1); 00756 00757 }else{ 00758 pc.printf("Could not connect to MPU9250: \n\r"); 00759 pc.printf("%#x \n", whoami); 00760 while(1) ; // Loop forever if communication doesn't happen 00761 } 00762 00763 mpu9250.getAres(); // Get accelerometer sensitivity 00764 mpu9250.getGres(); // Get gyro sensitivity 00765 mpu9250.getMres(); // Get magnetometer sensitivity 00766 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); 00767 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); 00768 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); 00769 magbias[0] = MAGBIASX; //+470.; // User environmental x-axis correction in milliGauss, should be automatically calculated 00770 magbias[1] = MAGBIASY; //+120.; // User environmental y-axis correction in milliGauss 00771 magbias[2] = MAGBIASZ; //+125.; // User environmental z-axis correction in milliGauss 00772 00773 t.start(); 00774 pc.printf("mpu9250 initialized\r\n"); 00775 00776 //---9軸センサー(MPU9250)初期化終わり--- 00777 00778 led1 = 0; 00779 led2 = 0; 00780 led3 = 0; 00781 led4 = 0; 00782 00783 pc.printf("All initialized\r\n"); 00784 00785 00786 while (true) { //無限ループ 00787 //for(i=0;i<8;i++){ 00788 //pc.printf("%x ", buf_sbus[i]); 00789 //pc.printf("ch%d=%d ", i+1, channels[i]); 00790 //} 00791 //pc.printf("\n"); 00792 sensing(); 00793 wait_ms(50); 00794 00795 00796 switch(OperationMode){ //変数OperationModeの値で場合分け 00797 case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行) 00798 if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら 00799 OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入 00800 pc.printf("go to autoloop\r\n"); 00801 led1 = 0; 00802 led2 = 0; 00803 }else{ 00804 led1 = 1; 00805 led2 = 1; 00806 } 00807 //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数 00808 update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す) 00809 // pc.printf("groundcheck mode\r\n"); 00810 00811 break; 00812 case AUTOLOOP: //水平旋回モード 00813 //t.reset(); 00814 if(CheckSW_up(7)){ //チャンネル7がonなら 00815 //t.reset(); 00816 Auto_Loop(); //関数Auto_Loop実行 00817 led1 = 1; 00818 led2 = 0; 00819 //Auto_Loop(); 00820 //pc.printf("Auto Loop Now\r\n"); 00821 //autoloop.set_priority(osPriorityAboveNormal); 00822 /* 00823 while(true){ 00824 if(!CheckSW_up(7)){ 00825 autoloop.terminate(); 00826 break; 00827 } 00828 } 00829 */ 00830 }else{ 00831 update_mode = UPD_MANUAL; 00832 //pc.printf("manual Now\r\n"); 00833 led1 = 0; 00834 led2 = 0; 00835 00836 /*debug*/ 00837 //k++; 00838 //wait(50); 00839 //if(k>100) pwm[6]=2000; 00840 } 00841 00842 if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら 00843 OperationMode=AUTOMOBIUS; 00844 pc.printf("go to auto8\r\n"); 00845 } 00846 break; 00847 case AUTOMOBIUS: //8の字旋回モード 00848 if(CheckSW_up(7)){ 00849 Auto_Mobius(); 00850 led1 = 1; 00851 led2 = 1; 00852 //pc.printf("Auto Mobius Now\r\n"); 00853 }else{ 00854 update_mode = UPD_MANUAL; 00855 //pc.printf("manual Now\r\n"); 00856 led1 = 0; 00857 led2 = 1; 00858 } 00859 00860 if(!CheckSW_up(7)&&!CheckSW_up(8)){ 00861 OperationMode=AUTOCLIMB; 00862 pc.printf("go to autoclimb\r\n"); 00863 } 00864 break; 00865 case AUTOCLIMB: //上昇旋回モード 00866 if(CheckSW_up(7)){ 00867 Auto_Climb(); 00868 led1 = 1; 00869 led2 = 0; 00870 //pc.printf("Auto Climb Now\r\n"); 00871 }else{ 00872 update_mode = UPD_MANUAL; 00873 //pc.printf("manual Now\r\n"); 00874 led1 = 0; 00875 led2 = 0; 00876 } 00877 00878 if(!CheckSW_up(7)&&CheckSW_up(8)){ 00879 OperationMode=AUTOGLIDE; 00880 pc.printf("go to manual\r\n"); 00881 } 00882 break; 00883 00884 case AUTOGLIDE: //自動滑空モード 00885 if(CheckSW_up(7)){ 00886 Auto_Glide(); 00887 led1 = 1; 00888 led2 = 1; 00889 //pc.printf("Auto Glide Now\r\n"); 00890 }else{ 00891 update_mode = UPD_MANUAL; 00892 //pc.printf("manual Now\r\n"); 00893 led1 = 0; 00894 led2 = 1; 00895 } 00896 00897 if(!CheckSW_up(7)&&!CheckSW_up(8)){ 00898 OperationMode=AUTOLANDING; 00899 pc.printf("go to autoloop\r\n"); 00900 } 00901 break; 00902 00903 case AUTOLANDING: 00904 led3 = 1; 00905 led4 = 1; //赤外線 00906 if(CheckSW_up(7)){ 00907 Auto_Landing(); 00908 led1 = 1; 00909 led2 = 0; 00910 //pc.printf("Auto Landing Now\r\n"); 00911 }else{ 00912 update_mode = UPD_MANUAL; 00913 //pc.printf("manual Now\r\n"); 00914 led1 = 0; 00915 led2 = 0; 00916 } 00917 00918 if(!CheckSW_up(7)&&CheckSW_up(8)){ 00919 OperationMode=AUTOLOOP; 00920 //pc.printf("go to manualmade\r\n"); 00921 } 00922 break; 00923 00924 case MANUALMODE: 00925 update_mode = UPD_MANUAL; 00926 led1 = 0; 00927 led2 = 1; 00928 led4 = 1; 00929 00930 if(!CheckSW_up(7)&&!CheckSW_up(8)){ 00931 OperationMode=AUTOLANDING; 00932 pc.printf("go to autolanding\r\n"); 00933 } 00934 break; 00935 00936 } 00937 00938 00939 00940 00941 /* 00942 if(CheckSW_up(7)){ 00943 pc.printf("true\r\n"); 00944 Auto_loop(); 00945 00946 }else{ 00947 pc.printf("false\r\n"); 00948 update_manual(); 00949 }*/ 00950 } 00951 }
Generated on Sat Jul 16 2022 12:57:01 by
1.7.2