MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
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秒ごと