Diff: main.cpp

Revision:

8:602865d8fca3

Parent:

6:2b68f85a984a

Child:

9:6d4578dcc1ed

--- a/main.cpp	Fri Jun 12 04:00:23 2015 +0000
+++ b/main.cpp	Fri Jun 12 15:28:05 2015 +0000
@@ -13,6 +13,10 @@
 /********** private define    **********/
 #define TRUE    1
 #define FALSE   0
+
+const float dt = 0.1f;              // 割り込み周期(s)
+const float ServoMax = 0.0023f;     // サーボの最大パルス長(s)
+const float ServoMin = 0.0006f;     // サーボの最小パルス長(s)
 /********** private macro     **********/
 
 /********** private typedef   **********/
@@ -23,33 +27,33 @@
 MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
 HMC5883L    hmc(&i2c);                  // 地磁気センサ
 LPS25H      lps(&i2c);                  // 気圧センサ
-Serial      gps(PA_11, PA_12);                // GPS通信用シリアルポート
+Serial      gps(PA_11, PA_12);          // GPS通信用シリアルポート
 Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
 GMS6_CR6    gms(&gps, &pc);             // GPS
-Ticker      INT_timer;                      // 割り込みタイマー
 Log         logger(PB_5, PB_4, PB_3, PB_10, PA_9, PA_10, PC_1);    // ロガー（microSD、XBee）
-PwmOut      servoL(PC_7), servoR(PB_6);
-AnalogIn    rf(PC_0);
-AnalogIn    servoVcc(PA_0);
-AnalogIn    logicVcc(PA_1);
-
-
-const float dt = 0.1f;           // 割り込み周期（s）
+PwmOut      servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
+AnalogIn    rf(PC_0);                   // 照度センサ用アナログ入力
+AnalogIn    servoVcc(PA_0);             // バッテリー電圧監視用アナログ入力(サーボ用)
+AnalogIn    logicVcc(PA_1);             // バッテリー電圧監視用アナログ入力(ロジック用)
+Ticker      INT_timer;                  // 割り込みタイマー
 
-int lps_cnt = 0;                    // 気圧センサ読み取りカウント
-uint8_t INT_flag = TRUE;            // 割り込み可否フラグ
-Vector raw_acc(3);                       // 加速度(m/s^2)  生
-Vector raw_gyro(3);                      // 角速度(deg/s)  生
-Vector raw_geomag(3);                    // 地磁気(?)  生
-float raw_press;                        // 気圧(hPa)  生
-Vector acc(3);                       // 加速度(m/s^2)
-Vector gyro(3);                      // 角速度(deg/s)
-Vector geomag(3);                    // 地磁気(?)
-float press;                        // 気圧(hPa)
+int lps_cnt = 0;                // 気圧センサ読み取りカウント
+uint8_t INT_flag = TRUE;        // 割り込み可否フラグ
+Vector raw_acc(3);              // 加速度(m/s^2)  生
+Vector raw_gyro(3);             // 角速度(deg/s)  生
+Vector raw_geomag(3);           // 地磁気(?)  生
+float raw_press;                // 気圧(hPa)  生
+Vector acc(3);                  // 加速度(m/s^2)
+Vector gyro(3);                 // 角速度(deg/s)
+Vector geomag(3);               // 地磁気(?)
+float press;                    // 気圧(hPa)
 
-Vector raw_g(3);                        // 重力ベクトル  生
-Vector g(3);                        // 重力ベクトル  生
-//Vector n(3);                        // 地磁気ベクトル
+Vector raw_g(3);                // 重力ベクトル  生
+Vector g(3);                    // 重力ベクトル
+//Vector n(3);                  // 地磁気ベクトル
+
+int pull_L = 0;                 // 左サーボの引っ張り量(mm：0~30)
+int pull_R = 0;                 // 右サーボの引っ張り量(mm：0~30)
 
 /* ----- Kalman Filter ----- */
 Vector pri_x(6);
@@ -86,6 +90,9 @@
     char* title = "log data\r\n";                                       // ログのタイトル行
     logger.initialize_sdlog(title);                                     // ログファイル初期設定
     
+    servoL.period(0.020f);                                              // サーボの信号の周期は20ms
+    servoR.period(0.020f);
+    
     KalmanInit();
     
     INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
@@ -105,6 +112,9 @@
         wait(0.05f); // 50 ms
         myled = 0; // LED is OFF
         
+        pull_L = (pull_L+5)%30;
+        pull_R = (pull_R+5)%30;
+        
         INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
         
         float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
@@ -114,7 +124,7 @@
                 g.GetComp(1), g.GetComp(2), g.GetComp(3), 
                 geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
                 press, gms.longitude, gms.latitude, 
-                sv, lv, gms.Ns);
+                sv, lv, rf.read_u16());
         logger.write(data);
         
         INT_flag = TRUE;            // 割り込み許可
@@ -122,8 +132,13 @@
         pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
         
         // 制御ルーチン
-        {
-        }
+        /*{
+            servoL.pulsewidth((ServoMax-ServoMin) * pull_L / 30.0f + ServoMin);
+            servoR.pulsewidth((ServoMax-ServoMin) * pull_R / 30.0f + ServoMin);
+            
+            
+            
+        }*/
         
         
         // ループはきっかり1秒ごと