勇輝 関本
４項目一応成功
Fork of
Estrela_v01
by 
Bot Furukawa
Diff: main.cpp
- Revision:
- 6:13bb74a57a33
- Parent:
- 5:c53ca479e96c
- Child:
- 7:13d98c4067c4
--- a/main.cpp Sun Aug 21 02:07:33 2016 +0000
+++ b/main.cpp Sun Aug 21 09:13:30 2016 +0000
@@ -8,6 +8,7 @@
#define UPD_MANUAL 0
#define UPD_AUTO 1
+#define UPD_LANDING 2
#define SBUS_SIGNAL_OK 0x00
#define SBUS_SIGNAL_LOST 0x01
@@ -40,6 +41,7 @@
uint8_t i,j,k;
uint8_t update_mode = 0;
static uint16_t pwm[8];
+static uint16_t oldTHL;
//9軸センサー関連
float sum = 0;
@@ -138,12 +140,12 @@
void update_pwm()
{
if(flg_ch_update == true){
- switch(update_mode){ //マニュアルモードか自動モードか切替
+ switch(update_mode){ //マニュアルモード,自動モード,自動着陸もモードを切替
case UPD_MANUAL: //マニュアルモード
for(j=0;j<8;j++){
pwm[j] = (channels[j]>>1) + 1000;
}
-
+ oldTHL = pwm[2];
jj=0;
//pc.printf("update_manual\r\n");
break;
@@ -161,6 +163,16 @@
//pc.printf("update_auto\r\n");
break;
+ case UPD_LANDING: //自動着陸モード
+ pwm[0] = (channels[0]>>1) + 1000;
+ pwm[1] = Auto_ELE;
+ pwm[2] = (channels[2]>>1) + 1000;
+ pwm[3] = (channels[3]>>1) + 1000;
+ pwm[4] = (channels[4]>>1) + 1000;
+ pwm[5] = (channels[5]>>1) + 1000;
+ pwm[6] = (channels[6]>>1) + 1000;
+ pwm[7] = (channels[7]>>1) + 1000;
+
default: //デフォはマニュアルモード
for(j=0;j<8;j++){
pwm[j] = (channels[j]>>1) + 1000;
@@ -171,7 +183,8 @@
break;
}
}
- flg_ch_update = false;
+ flg_ch_update = false;
+ ///oldTHL = pwm[2];
setall_servo();
}
@@ -299,6 +312,7 @@
}
+
//---sensing---
//センサーの値を読み込み、各種フィルタをかける(認知)
void sensing()
@@ -387,7 +401,7 @@
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("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100));
+ //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);
@@ -397,22 +411,60 @@
//}
}
+
+//---CalibrateCompass---
+//グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない
+void CalibrateCompass(void)
+{
+ led1 = 1;
+ sensing();
+ uint8_t ii;
+ float mxMAX=mx, mxMIN=mx;
+ float myMAX=my, myMIN=my;
+ magbias[0] = 0; //初期化
+ magbias[1] = 0;
+ //magbias[2] = 0;
+ ii=0;
+ while(1){ //キャリブレーション実行 led2が点滅時に機体を一回転させる xy方向のみ
+ sensing();
+ if(mxMAX < mx) mxMAX = mx;
+ if(mxMIN > mx) mxMIN = mx;
+ if(myMAX < my) myMAX = my;
+ if(myMIN > my) myMIN = my;
+ led2 = !led2;
+ ++ii;
+ wait(50);
+
+ if(!CheckSW_up(7)) break; //スイッチ7を下げて終了
+ }
+ led2 = 0;
+ if(ii<20){ //チャンネル7をすぐ切った場合 元に戻す
+ magbias[0] = MAGBIASX;
+ magbias[1] = MAGBIASY;
+ }else{ //しっかりと値を入れた場合
+ magbias[0] = (mxMAX+mxMIN)/2;
+ magbias[1] = (myMAX+myMIN)/2;
+ }
+ led1 = 0;
+ t.reset(); //タイマーをリセットして終了
+}
+
//---StabiGyro---
//水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
//右旋回
void StabiGyro()
{
- if(jj<=25){ //旋回し始め
- 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;
+ if(jj<=20){ //旋回し始め
+ Auto_RUD = RUD_N + -105 +int((-30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5
+ Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx
+ Auto_THR = oldTHL + 50;
//pc.printf("first%d\r\n",jj);
jj++;
}else{ //旋回中
- 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;
+ Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_ELE = ELE_N + -90 + int( (-18 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+ Auto_THR = oldTHL;
}
//pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする
@@ -436,13 +488,13 @@
void StabiGyro_Mobius()
{
if(jj<=25){ //右旋回導入
- Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN);
+ Auto_RUD = RUD_N + int((-25-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 + int((-17-roll)*RUDDER_GN - gy*RUD_DGN);
+ Auto_RUD = RUD_N + int((-20-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++;
@@ -453,12 +505,12 @@
//pc.printf("first%d\r\n",jj);
jj++;
}else if((117<jj) && (jj<=132)){ //旋回遷移2
- Auto_RUD = RUD_N + int((10-roll)*RUDDER_GN - gy*RUD_DGN);
+ Auto_RUD = RUD_N + int((25-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 + int((17-roll)*RUDDER_GN - gy*RUD_DGN);
+ Auto_RUD = RUD_N + int((20-roll)*RUDDER_GN - gy*RUD_DGN);
Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN);
Auto_THR = THR_N + 30;
}
@@ -483,13 +535,13 @@
void StabiGyro_Climb()
{
if(jj<=25){ //右旋回導入
- Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy
+ Auto_RUD = RUD_N + int((-25-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 if((25<jj) && (jj<=224)){ //右旋回途中
- Auto_RUD = RUD_N + int((-17-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75);
+ Auto_RUD = RUD_N + int((-20-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;
jj++;
@@ -700,7 +752,9 @@
if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら
OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
pc.printf("go to autoloop\r\n");
- }
+ }//else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル７だけonなら
+ //CalibrateCompass();
+ //}
//変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す)
// pc.printf("groundcheck mode\r\n");
@@ -792,12 +846,13 @@
}
if(!CheckSW_up(7)&&!CheckSW_up(8)){
- OperationMode=AUTOLOOP;
+ OperationMode=AUTOLANDING;
pc.printf("go to autoloop\r\n");
}
break;
- /*
+
case AUTOLANDING:
+ led3 = 1; //赤外線
if(CheckSW_up(7)){
Auto_Landing();
led1 = 0;
@@ -811,11 +866,11 @@
}
if(!CheckSW_up(7)&&CheckSW_up(8)){
- OperationMode=MANUALMODE;
- pc.printf("go to manualmade\r\n");
+ //OperationMode=MANUALMODE;
+ //pc.printf("go to manualmade\r\n");
}
break;
-
+ /*
case MANUALMODE:
update_mode = UPD_MANUAL;
led1 = 1;