Diff: main.cpp

Revision:

3:8c01b4f33389

Parent:

2:b88f73d7073c

Child:

4:ba610b05751d

--- a/main.cpp	Wed Aug 17 12:47:59 2016 +0000
+++ b/main.cpp	Sat Aug 20 01:44:54 2016 +0000
@@ -37,7 +37,7 @@
 static int16_t channels[18];
 uint8_t  failsafe_status = SBUS_SIGNAL_FAILSAFE;
 static bool flg_ch_update = false;
-uint8_t i,j;
+uint8_t i,j,k;
 uint8_t update_mode = 0;
 static uint16_t pwm[8];
 
@@ -356,9 +356,9 @@
     //pc.printf(" gy = %f", gy); 
     //pc.printf(" gz = %f  deg/s\n\r", gz); 
     
-    //pc.printf("gx = %f", mx); 
-    //pc.printf(" gy = %f", my); 
-    //pc.printf(" gz = %f  mG\n\r", mz); 
+    //pc.printf("mx = %f", mx); 
+    //pc.printf(" my = %f", my); 
+    //pc.printf(" mz = %f  mG\n\r", mz); 
     
     tempCount = mpu9250.readTempData();  // Read the adc values
     temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
@@ -387,7 +387,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("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
+    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(); 
@@ -556,18 +556,21 @@
 
 void Auto_Loop()
 {
-    //while(true){
+    while(true){
+    //for(k=0;k<100;++k){
         sensing();      //センサーの値を読み込む(認知)
         StabiGyro();    //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
         update_mode = UPD_AUTO;     //オートモード
-        
         //StabiAccel();
         //Auto_ELE+=145;
         //Auto_RUD-=80;
         //Servos.Auto2Servo();
         //Thread::wait(50);
+        if(!CheckSW_up(7)) break;
         wait_ms(50);        
-    //}   
+    }   
+    k=0; //debug
+    pwm[6]=1000;
 }
 
 void Auto_Mobius()
@@ -661,10 +664,11 @@
     //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
     //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
     //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
-    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
-    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
-    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+    magbias[0] = MAGBIASX;       //+470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
+    magbias[1] = MAGBIASY;       //+120.;  // User environmental y-axis correction in milliGauss
+    magbias[2] = MAGBIASZ;       //+125.;  // User environmental z-axis correction in milliGauss
     
+    t.start();
     pc.printf("mpu9250 initialized\r\n");
     
     //---9軸センサー(MPU9250)初期化終わり---   
@@ -676,7 +680,7 @@
     
     pc.printf("All initialized\r\n");
     
-    
+
     while (true) {      //無限ループ
         //for(i=0;i<8;i++){
         //pc.printf("%x ", buf_sbus[i]);
@@ -685,7 +689,6 @@
         //pc.printf("\n");
         //wait_ms(500); 
         
-        
         switch(OperationMode){      //変数OperationModeの値で場合分け
             case GROUNDCHECK:   //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行)
                 if(!CheckSW_up(7)&&!CheckSW_up(8)){     //チャンネル7,8が両方offなら
@@ -698,11 +701,14 @@
                 
                 break;
             case AUTOLOOP:      //水平旋回モード
+                t.reset();
                 if(CheckSW_up(7)){  //チャンネル7がonなら
-                    Auto_Loop();    //関数Auto_Loop実行
+                    t.reset();
+                    //Auto_Loop();    //関数Auto_Loop実行
                     led1 = 0;
                     led2 = 1;
-                    pc.printf("Auto Loop Now\r\n");
+                    Auto_Loop();
+                    //pc.printf("Auto Loop Now\r\n");
                     //autoloop.set_priority(osPriorityAboveNormal);
                     /*
                     while(true){
@@ -717,6 +723,12 @@
                     pc.printf("manual Now\r\n");
                     led1 = 1;
                     led2 = 0;
+                    
+                    //debug
+                    k++;
+                    wait(50);
+                    //if(k>100) pwm[6]=2000;
+                        
                 }  
                 
                 if(!CheckSW_up(7)&&CheckSW_up(8)){  //チャンネル7がoff、8がonなら