勇輝 関本
４項目一応成功
Fork of
Estrela_v01
by 
Bot Furukawa
Diff: main.cpp
- Revision:
- 4:ba610b05751d
- Parent:
- 3:8c01b4f33389
- Child:
- 5:c53ca479e96c
--- 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なら