hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Diff: main.cpp
- Revision:
- 33:56163d4e2c53
- Parent:
- 32:b35efe3d1091
- Child:
- 34:4bda9af9a0cd
--- a/main.cpp Sun Jul 12 04:36:14 2015 +0000
+++ b/main.cpp Sun Jul 19 02:21:43 2015 +0000
@@ -26,9 +26,10 @@
const float dt = 0.01f; // 割り込み周期(s)
const float ServoMax = 0.0046f; // サーボの最大パルス長(s)
const float ServoMin = 0.0012f; // サーボの最小パルス長(s)
-const float Stabilize = 0; // 滑空安定時の引っ張り量
-const float Turn = 25; // 旋回時の引っ張り量
-const float PullMax = 25; // 引っ張れる紐の最大量(mm)
+const float Stabilize = 0.0f; // 滑空安定時の引っ張り量
+const float Turn = 25.0f; // 旋回時の引っ張り量
+const float PullRate = 5.0f; // 紐の引っ張り速度
+const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
const float BorderSpiral = 40.0f; // スパイラル検知角度
const short BorderOpt = 30000; // 光センサーの閾値
const float BorderGravity = 0.3f; // 無重力状態の閾値
@@ -57,7 +58,7 @@
FILE * fp; // ログファイルのポインタ
BufferedSerial xbee(PA_9, PA_10, PC_1, 256, 16); // Xbee
ConfigFile cfg; // ConfigFile
-PwmOut servoL(PC_7), servoR(PB_6); // サーボ用PWM出力
+PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
AnalogIn servoVcc(PA_1); // バッテリー電圧監視用アナログ入力(サーボ用)
AnalogIn logicVcc(PA_0); // バッテリー電圧監視用アナログ入力(ロジック用)
@@ -101,8 +102,8 @@
Vector r_u(3); // 世界座標に固定された、上向きのベクトル（Z軸）
Vector r_l(3); // 世界座標に固定された、西向きのベクトル（Y軸）
-int pull_L = 0; // 左サーボの引っ張り量(mm：0~PullMax)
-int pull_R = 0; // 右サーボの引っ張り量(mm：0~PullMax)
+float pull_L = 0.0f; // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
+float pull_R = 0.0f; // 右サーボの引っ張り量（目標値）(mm：0~PullMax)
float yaw = 0.0f; // 本体のヨー角（deg）z軸周り
float pitch = 0.0f; // 本体のピッチ角（deg）y軸周り
@@ -241,13 +242,13 @@
if(xbee.readable()) {
char cmd = xbee.getc();
if(cmd == 'w') {
- pull_R = Turn;
+ pull_R += 5.0f;
} else if(cmd == 's'){
- pull_R = 0;
+ pull_R -= 5.0f;
} else if(cmd == 'a'){
- pull_L = Turn;
+ pull_L += 5.0f;
} else if(cmd == 'd'){
- pull_L = 0;
+ pull_L -= 5.0f;
}
}
#else
@@ -257,14 +258,7 @@
sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧
lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧
- // 指定された引っ張り量だけ紐を引っ張る
- if(pull_L < 0) pull_L = 0;
- else if(pull_L > PullMax) pull_L = PullMax;
- if(pull_R < 0) pull_R = 0;
- else if(pull_R > PullMax) pull_R = PullMax;
-
- servoL.pulsewidth((ServoMax - ServoMin) * (PullMax - pull_L) / (float)PullMax + ServoMin);
- servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin);
+
// データをmicroSDに保存し、XBeeでPCへ送信する
sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
@@ -378,8 +372,8 @@
pull_L = Stabilize;
pull_R = Stabilize;
if(count >= WaitTime*5) {
- pull_L = 0;
- pull_R = 0;
+ pull_L = 0.0f;
+ pull_R = 0.0f;
#ifdef SPIRAL_DEBUG
step = 2;
#else
@@ -395,9 +389,9 @@
// スパイラル回避行動
/*if(roll > 0) {
pull_L = PullMax;
- pull_R = 0;
+ pull_R = 0.0f;
} else {
- pull_L = 0;
+ pull_L = 0.0f;
pull_R = PullMax;
}*/
@@ -466,16 +460,16 @@
if(dir == LEFT) { //目標は左方向
pull_L = Turn;
- pull_R = 0;
+ pull_R = 0.0f;
} else if (dir == RIGHT) { //目標は右方向
- pull_L = 0;
+ pull_L = 0.0f;
pull_R = Turn;
} else if (dir == NEUTRAL) {
- pull_L = 0;
- pull_R = 0;
+ pull_L = 0.0f;
+ pull_R = 0.0f;
}
}
#ifdef RULE4
@@ -495,7 +489,7 @@
#if defined(RULE2) || defined(RULE4) || defined(SPIRAL_DEBUG)
// 落下シーケンス
case 2:
- pull_L = 0;
+ pull_L = 0.0f;
pull_R = PullMax;
#ifndef SPIRAL_DEBUG
@@ -525,7 +519,7 @@
// 機体に固定されたベクトルの初期化
b_f.SetComp(1, 0.0f);
b_f.SetComp(2, 0.0f);
- b_f.SetComp(3, 1.0f);
+ b_f.SetComp(3, -1.0f);
b_u.SetComp(1, 1.0f);
b_u.SetComp(2, 0.0f);
b_u.SetComp(3, 0.0f);
@@ -536,7 +530,7 @@
target_p.SetComp(2, target_y * DEG_TO_RAD);
// 地表の気圧を取得
- p0 = 0;
+ p0 = 0.0f;
for(int i=0; i<10; i++) {
p0 += (float)lps.pressure() * PRES_LSB_TO_HPA;
wait(0.1f);
@@ -821,6 +815,32 @@
*/
void DataUpdate()
{
+ // サーボ用変数（引き量）
+ static float pL = pull_L;
+ static float pR = pull_R;
+
+ // 引き量が目標値から一定以上離れていれば、引き量を一定の速度で更新する
+ if(pull_L - pL > PullRate * dt) {
+ pL += PullRate * dt;
+ } else if(pull_L - pL < -PullRate * dt) {
+ pL -= PullRate * dt;
+ }
+
+ if(pull_R - pR > PullRate * dt) {
+ pR += PullRate * dt;
+ } else if(pull_R - pR < -PullRate * dt) {
+ pR -= PullRate * dt;
+ }
+
+ // 指定された引っ張り量だけ紐を引っ張る
+ if(pL < 0.0f) pL = 0.0f;
+ else if(pL > PullMax) pL = PullMax;
+ if(pR < 0.0f) pR = 0.0f;
+ else if(pR > PullMax) pR = PullMax;
+
+ servoL.pulsewidth((ServoMax - ServoMin) * (PullMax - pL) / PullMax + ServoMin);
+ servoR.pulsewidth((ServoMax - ServoMin) * pR / PullMax + ServoMin);
+
// センサーの値を更新
mpu.read();
hmc.read();