![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
4項目一応成功
Fork of Estrela_v01 by
Diff: main.cpp
- Revision:
- 4:ba610b05751d
- Parent:
- 3:8c01b4f33389
- Child:
- 5:c53ca479e96c
diff -r 8c01b4f33389 -r ba610b05751d main.cpp --- a/main.cpp Sat Aug 20 01:44:54 2016 +0000 +++ b/main.cpp Sun Aug 21 01:39:04 2016 +0000 @@ -124,7 +124,7 @@ }*/ /* for(j=0;j<8;j++){ - pc.printf("ch%d=%d ", j+1, channels[j]); + pc.printf("ch%d=%d ", j+1, pwm[j]); } //pc.printf("time = %d", frq.read_ms()); pc.printf("\r\n"); @@ -145,7 +145,7 @@ } jj=0; - pc.printf("update_manual\r\n"); + //pc.printf("update_manual\r\n"); break; case UPD_AUTO: //自動モード @@ -380,49 +380,51 @@ // applied in the correct order which for this configuration is yaw, pitch, and then roll. // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 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]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - roll = 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]); + roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); + 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]); pitch *= 180.0f / PI; yaw *= 180.0f / PI; yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 roll *= 180.0f / PI; - pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll); + pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100)); + //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll); //pc.printf("average rate = %f\n\r", (float) sumCount/sum); count = t.read_ms(); sum = 0; sumCount = 0; //} - } + //---StabiGyro--- //水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断) +//右旋回 void StabiGyro() { if(jj<=25){ //旋回し始め - Auto_RUD = RUD_N - 30 - int((gy+130)*0.75); //roll -> gy - Auto_ELE = ELE_N + 30 - int((gx-30)*0.8); //pitch -> -gx + Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy + Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx Auto_THR = THR_N + 50; //pc.printf("first%d\r\n",jj); jj++; }else{ //旋回中 - Auto_RUD = RUD_N - 20 - int((gy+25)*0.75); - Auto_ELE = ELE_N - 10 - int((gx-30)*0.8); + Auto_RUD = RUD_N + int((-17-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN); Auto_THR = THR_N; } //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする - if(Auto_RUD > 1790){ - Auto_RUD = 1790; - }else if(Auto_RUD < 1032){ - Auto_RUD = 1032; + if(Auto_RUD > RUD_MAX){ + Auto_RUD = RUD_MAX; + }else if(Auto_RUD < RUD_MIN){ + Auto_RUD = RUD_MIN; } - if(Auto_ELE > 1690){ - Auto_ELE = 1690; - }else if(Auto_ELE < 1192){ - Auto_ELE = 1192; + if(Auto_ELE > ELE_MAX){ + Auto_ELE = ELE_MAX; + }else if(Auto_ELE < ELE_MIN){ + Auto_ELE = ELE_MIN; } //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); @@ -434,42 +436,42 @@ void StabiGyro_Mobius() { if(jj<=25){ //右旋回導入 - Auto_RUD = RUD_N - 30 - int((gy+130)*0.75); - Auto_ELE = ELE_N + 30 - int((gx-30)*0.8); - Auto_THR = THR_N + 50; + Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); + Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = THR_N + 50; //pc.printf("first%d\r\n",jj); jj++; }else if((25<jj) && (jj<=112)){ //右旋回途中 - Auto_RUD = RUD_N - 20 - int((gy+25)*0.75); - Auto_ELE = ELE_N - 10 - int((gx-30)*0.8); + Auto_RUD = RUD_N + int((-17-roll)*RUDDER_GN - gy*RUD_DGN); + Auto_ELE = ELE_N + int( (LAND_ANGLE - 1 - pitch)*ELEVATOR_GN - gx*ELE_DGN); Auto_THR = THR_N; jj++; }else if((112<jj) && (jj<=117)){ //旋回遷移1 - Auto_RUD = RUD_N + 160 - int((gy-100)*0.75); - Auto_ELE = ELE_N + 20 - int((gx+0)*0.6); - Auto_THR = THR_N - 100; + Auto_RUD = RUD_N + int((-roll)*RUDDER_GN - gy*RUD_DGN); + Auto_ELE = ELE_N + int( (LAND_ANGLE + 2 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = THR_N - 100; //pc.printf("first%d\r\n",jj); jj++; }else if((117<jj) && (jj<=132)){ //旋回遷移2 - Auto_RUD = RUD_N + 120 - int((gy-100)*0.75); - Auto_ELE = ELE_N + 65 - int((gx-30)*0.6); - Auto_THR = THR_N - 20; + Auto_RUD = RUD_N + int((10-roll)*RUDDER_GN - gy*RUD_DGN); + Auto_ELE = ELE_N + int( (LAND_ANGLE + 6 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = THR_N - 20; jj++; }else{ //左旋回途中 - Auto_RUD = RUD_N + 120 - int((gy-25)*0.75); - Auto_ELE = ELE_N + 0 - int((gx-30)*0.6); - Auto_THR = THR_N + 30; + Auto_RUD = RUD_N + int((17-roll)*RUDDER_GN - gy*RUD_DGN); + Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = THR_N + 30; } - if(Auto_RUD > 1790){ - Auto_RUD = 1790; - }else if(Auto_RUD < 1032){ - Auto_RUD = 1032; + if(Auto_RUD > RUD_MAX){ + Auto_RUD = RUD_MAX; + }else if(Auto_RUD < RUD_MIN){ + Auto_RUD = RUD_MIN; } - if(Auto_ELE > 1690){ - Auto_ELE = 1690; - }else if(Auto_ELE < 1192){ - Auto_ELE = 1192; + if(Auto_ELE > ELE_MAX){ + Auto_ELE = ELE_MAX; + }else if(Auto_ELE < ELE_MIN){ + Auto_ELE = ELE_MIN; } //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); @@ -508,15 +510,15 @@ Auto_THR = THR_N; } - if(Auto_RUD > 1790){ - Auto_RUD = 1790; - }else if(Auto_RUD < 1032){ - Auto_RUD = 1032; + if(Auto_RUD > RUD_MAX){ + Auto_RUD = RUD_MAX; + }else if(Auto_RUD < RUD_MIN){ + Auto_RUD = RUD_MIN; } - if(Auto_ELE > 1690){ - Auto_ELE = 1690; - }else if(Auto_ELE < 1192){ - Auto_ELE = 1192; + if(Auto_ELE > ELE_MAX){ + Auto_ELE = ELE_MAX; + }else if(Auto_ELE < ELE_MIN){ + Auto_ELE = ELE_MIN; } //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); @@ -539,15 +541,15 @@ Auto_THR = 1000; //スロットルoff //} - if(Auto_RUD > 1790){ - Auto_RUD = 1790; - }else if(Auto_RUD < 1032){ - Auto_RUD = 1032; + if(Auto_RUD > RUD_MAX){ + Auto_RUD = RUD_MAX; + }else if(Auto_RUD < RUD_MIN){ + Auto_RUD = RUD_MIN; } - if(Auto_ELE > 1690){ - Auto_ELE = 1690; - }else if(Auto_ELE < 1192){ - Auto_ELE = 1192; + if(Auto_ELE > ELE_MAX){ + Auto_ELE = ELE_MAX; + }else if(Auto_ELE < ELE_MIN){ + Auto_ELE = ELE_MIN; } //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE); @@ -556,9 +558,9 @@ void Auto_Loop() { - while(true){ + //while(true){ //for(k=0;k<100;++k){ - sensing(); //センサーの値を読み込む(認知) + //sensing(); //センサーの値を読み込む(認知) StabiGyro(); //水平旋回のためのラダー、エレベーターのpwmを計算(判断) update_mode = UPD_AUTO; //オートモード //StabiAccel(); @@ -566,11 +568,15 @@ //Auto_RUD-=80; //Servos.Auto2Servo(); //Thread::wait(50); - if(!CheckSW_up(7)) break; - wait_ms(50); - } - k=0; //debug - pwm[6]=1000; + /* + if(!CheckSW_up(7)){ + pc.printf("go to manual\n"); + break; + }*/ + //wait_ms(50); + //} + //k=0; //for debug + //pwm[6]=1000; } void Auto_Mobius() @@ -687,7 +693,8 @@ //pc.printf("ch%d=%d ", i+1, channels[i]); //} //pc.printf("\n"); - //wait_ms(500); + sensing(); + wait_ms(50); switch(OperationMode){ //変数OperationModeの値で場合分け case GROUNDCHECK: //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行) @@ -697,17 +704,17 @@ } //変数update_modeはSBus(=プロポ=操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数 update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す) - pc.printf("groundcheck mode\r\n"); + // pc.printf("groundcheck mode\r\n"); break; case AUTOLOOP: //水平旋回モード - t.reset(); + //t.reset(); if(CheckSW_up(7)){ //チャンネル7がonなら - t.reset(); - //Auto_Loop(); //関数Auto_Loop実行 + //t.reset(); + Auto_Loop(); //関数Auto_Loop実行 led1 = 0; led2 = 1; - Auto_Loop(); + //Auto_Loop(); //pc.printf("Auto Loop Now\r\n"); //autoloop.set_priority(osPriorityAboveNormal); /* @@ -720,15 +727,14 @@ */ }else{ update_mode = UPD_MANUAL; - pc.printf("manual Now\r\n"); + //pc.printf("manual Now\r\n"); led1 = 1; led2 = 0; - //debug - k++; - wait(50); - //if(k>100) pwm[6]=2000; - + /*debug*/ + //k++; + //wait(50); + //if(k>100) pwm[6]=2000; } if(!CheckSW_up(7)&&CheckSW_up(8)){ //チャンネル7がoff、8がonなら